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I.     INTRODUCTION 

A.  BACKGROUND  AND  LITERATURE  SURVEY 

Robots  are  presently  an  integral  part  of  industrial  processes  They  perform  tasks  with 
high  precision,  speed  and  reliability.  These  same  features  make  robots  attractive  with 
regards  to  space  applications. 

Space  based  robotics  platforms  experience  conditions  unlike  those  of  their  terrestrial 
counterparts.  With  respect  to  the  dynamics  of  the  systems,  the  most  notable  difference  is 
the  absence  of  a  fixed  base  on  which  to  locate  the  manipulators.  The  consequence  of  the 
difference  is  that  the  motion  of  the  space  based  manipulator  transmits  forces  and  moments 
to  its  mounting  base  resulting  in  translation  and  rotation  of  the  base  itself.  This  of  course 
impacts  the  location  of  the  manipulator's  end  effector.  The  problem  is  further  complicated 
in  that  the  disturbances  are  not  simply  a  function  of  the  present  manipulator  joint  angles 
but  are  also  a  function  of  the  joint  angle  histories  preceding  the  current  configuration. 
(Ref  1) 

A  number  of  approaches  have  been  used  for  dealing  with  this  coupling  of  joint  angle 
histories  and  spacecraft  main  body  attitude.  Wang  (Ref.  2)  eliminates  the  problem  by 
carefully  defining  what  he  expects  of  his  dual-arm  maneuverable  space  robot.  He  preposi- 
tions the  manipulators  such  that  they  are  configured  to  grasp  the  payload  once  the  vehicle 
moves  within  range.  After  the  manipulators  are  in  position,  their  joints  are  locked  while 
the  spacecraft  maneuvers  to  a  location  and  attitude  near  the  payload.  Next,  the  vehicle 
approaches  the  payload  in  a  straight  line  until  the  end  effectors  can  grasp  the  payload. 
While  the  manipulator  joints  remain  locked,  the  vehicle  repositions  the  entire  rigid  body 
system  to  the  desired  payload  destination.    At  this  point,  the  payload  is  released  and  the 


vehicle  backs  away  along  a  straight  line.  The  repositioning  of  the  payload  is  accom- 
plished by  means  of  the  vehicle  attitude  control  rather  than  altering  the  joint  angles  in  the 
manipulators.  The  manipulators  themselves  are  not  moved  except  when  the  attitude  dis- 
turbance they  impart  to  the  vehicle  is  of  no  importance.  Maintaining  vehicle  attitude  dur- 
ing manipulator  motion  is  not  a  requirement. 

Longman,  Lindberg  and  Zedd  (Ref.  3)  calculate  the  disturbance  torques  caused  by 
manipulator  motion.  This  information  is  used  to  calculate  reaction  wheel  commands 
which  will  compensate  for  the  disturbance  torques.  In  this  way,  spacecraft  attitude  control 
is  maintained  while  the  manipulator  is  repositioned. 

If  the  vehicle  does  not  contain  reaction  wheels,  the  primary  source  of  attitude  control 
is  probably  reaction  control  thrusters  Because  fuel  is  consumable  and  hence  mission  lim- 
iting, firing  thrusters  to  hold  spacecraft  attitude  should  be  avoided  whenever  possible. 
Vafa  and  Dubowsky  (Ref.  4)  and  Longman  (Ref.  5)  use  similar  approaches  to  eliminate 
the  need  for  reaction  thruster  firings.  Both  techniques  involve  constructing  a  manipulator 
trajectory  which  involves  revolving  the  manipulator  in  a  small  coning  motion  at  interme- 
diate stages  of  the  payload  repositioning  maneuver.  This  motion  imparts  a  slow  rotation 
of  the  spacecraft  about  the  coning  axis.  Careful  use  of  the  coning  locations  permits  repo- 
sitioning of  the  payload  between  any  arbitrary  locations  (within  manipulator  reach)  and 
attitudes  while  also  changing  the  spacecraft  to  any  desired  attitude  without  the  need  for 
thruster  firings.  This  technique  does  not,  however,  maintain  a  particular  spacecraft  atti- 
tude during  the  maneuver. 

Nakamura  and  Mukherjee  (Ref.  6)  use  a  technique  called  the  bi-directional  approach. 
This  method  represents  a  six  degree  of  freedom  (DOF)  manipulator  mounted  on  a  space 
vehicle  as  a  nine  variable  system  (six  joint  angles  and  three  spacecraft  attitude  angles) 
with  six  inputs  (the  manipulator  joint  torques).  They  attack  the  problem  from  both  ends 
by  integrating  forward  from  the  initial  conditions  and  backwards  from  the  desired  final 


conditions.  A  Lyapunov  function  guarantees  that  the  two  solutions  will  converge  at  some 
intermediate  time  during  the  maneuver.  As  in  Ref.  4  and  Ref.  5,  the  payload  is  reposi- 
tioned to  the  desired  location  and  attitude  and  the  attitude  of  the  spacecraft  main  body  is 
changed  to  its  desired  orientation.  However,  the  main  body  attitude  during  the  maneuver 
is  not  controlled.  Furthermore,  the  joint  angle  trajectories  calculated  by  this  method  con- 
tain rapid,  large  oscillations  near  the  maneuver  start  and  stop  times  and  noncontinuous 
derivatives  at  the  convergence  time. 

Vafa  (Ref.  7)  succeeds  in  using  a  single  space-based  manipulator  to  control  spacecraft 
attitude  during  a  repositioning  maneuver.  He  does  this  by  employing  a  nine  DOF  manipu- 
lator. This  manipulator  has  enough  redundancy  in  its  kinematics  to  control  the  end  effec- 
tor location  and  attitude  and  the  spacecraft  attitude.  Six  DOF  are  allocated  to 
repositioning  the  payload  and  the  remaining  three  DOF  are  used  to  control  the  spacecraft 
attitude. 

Like  Ref.  7,  the  primary  objective  of  Chung,  Desa  and  deSilva  (Ref.  8)  is  to  address 
the  disturbances  transmitted  to  the  spacecraft  by  the  manipulator  motion.  They  also  use  a 
single  manipulator  with  redundant  kinematics.  Because  they  use  inverse  kinematics  to 
find  the  joint  torques,  the  manipulator  redundancy  prevents  the  existence  of  a  unique  solu- 
tion. A  solution  is  selected  from  among  the  infinity  of  possible  solutions  by  means  of 
minimizing  a  cost  function  of  the  magnitudes  of  the  forces  and  torques  transmitted  to  the 
base. 

Torres  and  Dubowsky  (Ref.  9)  also  focus  on  the  spacecraft  attitude  disturbances 
caused  by  manipulator  motion.  They  recognize  that  for  any  given  point  in  joint  space, 
there  is  a  direction  of  motion  which  produces  minimum  spacecraft  attitude  disturbance 
and  a  perpendicular  direction  of  motion  which  produces  maximum  spacecraft  attitude  dis- 
turbance. A  tool  called  the  enhanced  disturbance  map  (EDM)  depicts  these  directions 
graphically.    The  EDM  permits  users  to  plan  manipulator  trajectories  that  lie  on  or  near 


zero  disturbance  paths.  For  non  redundant  manipulators,  this  may  involve  repositioning 
the  spacecraft  itself  prior  to  the  manipulator  repositioning  maneuver.  If  the  manipulator 
has  redundant  kinematics,  one  can  find  a  zero  disturbance  path  to  connect  the  manipulator 
trajectory  endpoints  without  having  to  reposition  the  spacecraft  initially.  This  technique 
considers  only  the  location  of  the  manipulator  endpoint  and  not  its  attitude. 

Configurations  with  multiple  manipulators  have  also  been  explored.  The  closed  chain 
nature  of  these  configurations  prevent  the  use  of  some  of  the  techniques  already  discussed. 
In  addition,  controlling  multiple  manipulators  raises  the  issue  of  cooperation  between  the 
manipulators. 

Nguyen,  Pooran  and  Premack  (Ref.  10)  develop  a  PD  controller  for  a  fixed  base,  two 
DOF,  closed  chain  manipulator  system.  The  system  is  linearized  by  means  of  Taylor 
series  expansion  about  a  point  designated  as  the  robot's  "home"  point.  Pole  placement  is 
then  used  to  select  controller  gains. 

Hu  and  Goldenberg  (Ref.  11)  derive  an  adaptive  control  scheme  for  multiple  nonre- 
dundant  manipulators  mounted  to  a  fixed  base.  In  this  reference,  coordinated  control 
involves  controlling  the  motion  of  the  grasped  object,  the  contact  forces  between  the 
object  and  its  environment,  and  the  internal  forces  within  the  object  caused  from  being 
held  by  more  than  one  manipulator. 

For  a  space  based  system,  contact  forces  between  the  payload  and  its  environment  are 
less  likely  to  be  important.  Walker,  Kim  and  Dionise  (Ref.  12)  present  just  such  an  adap- 
tive controller. 

Coordinated  control  of  multiple  manipulators  assumes  a  different  meaning  according 
to  Yoshida,  Kurazume  and  Umetani  (Ref.  13).  Although  they  propose  a  system  with  two 
manipulators,  only  one  actually  grasps  the  payload.  The  other  is  used  to  provide  counter- 
balancing torques  to  the  spacecraft  main  body.  The  role  of  the  second  manipulator  is  sim- 


ilar  to  a  reaction  wheel  in  that  its  primary  function  is  to  control  spacecraft  attitude  rather 
than  reposition  a  payload. 

Ahmad  and  Zribi  (Ref.  14)  apply  a  Lyapunov  controller  to  a  fixed  base,  multiple 
manipulator  system.  As  in  Ref.  12,  they  are  concerned  with  controlling  the  payload  posi- 
tion and  its  internal  forces.  To  do  so,  the  method  requires  sensors  to  measure  the  forces 
and  moments  created  by  each  manipulator.  They  also  present  an  adaptive  version  to  con- 
trol this  system. 

While  still  addressing  payload  position  and  internal  forces,  Schneider  and  Cannon 
(Ref.  15)  use  a  technique  called  object  impedance  control  to  achieve  coordinated  control 
among  the  manipulators.  This  method  views  the  payload  as  being  anchored  to  a  desired 
location  by  a  spring/damper  system. 

B.  DISSERTATION  OVERVIEW  AND  OBJECTIVES 

This  research  is  concerned  with  the  cooperative  control  of  a  space  based  manipulator 
system  with  multiple  manipulators  handling  a  common  payload.  The  scope  is  limited  to 
planar  motion  in  which  the  spacecraft  is  allowed  to  rotate  but  not  to  translate.  These 
restrictions  permit  experimental  verification  in  the  Spacecraft  Dynamics  and  Control  Lab- 
oratory at  the  Naval  Postgraduate  School.  The  objectives  of  this  research  are  to  1)  develop 
a  stable  control  law  which  facilitates  cooperation  among  the  manipulators  as  they  reposi- 
tion the  payload,  2)  minimize  joint  actuator  effort,  3)  reduce  the  disturbance  torque  trans- 
mitted to  the  spacecraft  main  body  by  the  manipulator  motion,  and  4)  validate  the 
analytical  development  with  experimental  results. 

Chapter  II  develops  the  analytical  model  in  detail.  Coordinate  systems  are  defined  and 
the  equations  of  motion  are  derived.  A  technique  for  finding  control  torques  which  mini- 
mizes a  weighted  norm  is  presented.    A  globally  stable  control  law  is  developed  using 


Lyapunov  methods.  The  idea  of  using  a  reference  trajectory  to  describe  the  motion  is 
applied  as  are  methods  for  choosing  the  reference  trajectory. 

Chapter  III  verifies  the  analytical  model  with  several  test  cases.  The  model  is  evalu- 
ated for  compliance  with  the  principles  of  conservation  of  kinetic  energy  and  angular 
momentum.  After  establishing  the  validity  of  the  model,  results  from  simulations  are  pre- 
sented. The  stability  of  the  controller  is  illustrated  as  is  the  dramatic  improvement  in  per- 
formance when  a  reference  trajectory  is  included  Results  from  a  simplified  control  law 
which  is  more  practical  to  implement  are  included  and  compared  to  the  complete  control 
law  version. 

Discussion  of  the  experimental  work  is  contained  in  Chapter  IV.  This  chapter  includes 
a  description  of  the  experimental  setup.  As  might  be  expected,  actual  hardware  demon- 
strated that  there  are  differences  between  the  ideal  world  of  the  analytical  model  and  the 
real  world  of  hardware  implementation. 

The  summary  and  concluding  remarks  are  presented  in  Chapter  V.  This  chapter  also 
contains  suggestions  for  future  work  in  this  field. 


II.    ANALYTICAL  MODEL 

The  analytical  model  represents  a  spacecraft  with  two  manipulators  attached.  The 
manipulators  have  already  firmly  grasped  an  object  hereafter  referred  to  as  the  payload. 
The  manipulators  are  about  to  reposition  the  payload  with  respect  to  the  spacecraft.  The 
ensuing  dynamics  between  the  spacecraft,  manipulators,  and  payload  are  the  topic  of  this 
research.  What  occurs  before  the  manipulators  grasp  the  payload  and  after  they  release  it 
is  beyond  the  scope  of  this  investigation.  The  scope  is  narrowed  further  by  confining  the 
motion  to  be  two  dimensional  and  allowing  the  spacecraft  to  rotate  but  not  translate. 
These  assumptions  are  consistent  with  hardware  restrictions  during  experimental  verifica- 
tion. 

A.  COORDINATE  SYSTEMS 

Figure  1  shows  a  schematic  of  the  overall  system.  This  diagram  illustrates  the  rela- 
tionship between  the  coordinate  frames  used  to  develop  the  equations  of  motion.  All 
angles  are  measured  positive  counterclockwise.  The  centerbody,  manipulator  links,  and 
payload  are  assumed  to  be  rigid  bodies.  Therefore,  member  lengths  (^j,  ^L2'  ^Rb  ^R2^  and 
/p),  distances  to  member  centers  of  mass  (ZcU,  lc\  ],  lc\2,  A;Rb  ^cR2'  and  ^ch)>  ar»d the  loca- 
tion of  the  left  and  right  shoulders  (£L0,  9{  0,  ^(),  and  9R0)  remain  constant.  An  inertial 
axis  system  is  located  on  the  centerbody  at  the  point  of  rotation.  A  body  fixed  coordinate 
frame  uses  the  same  origin  as  the  inertial  frame  but  rotates  with  the  spacecraft  centerbody. 
The  x-axis  of  this  frame  points  to  the  centerbody  center  of  mass.  The  centerbody  attitude, 
0O,  is  the  angle  between  the  inertial  x-axis  and  the  spacecraft  centerbody  x-axis.  Each 
manipulator  link  has  its  own  set  of  body  axes.    A  coordinate  frame  attached  to  the  left 


->•      Inertial  Axes 


-O      Body  Axes 


Figure  1:  Dual  Two-Link  Manipulator  Configuration 
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shoulder  aligns  its  x-axis  along  the  longitudinal  axis  of  manipulator  Link  LI .  The  attitude 
of  this  link,  0L  j,  is  zero  when  the  link  lies  on  a  ray  extending  from  the  inertial  origin 
through  the  left  shoulder.  The  attitude  of  Link  L2  is  defined  by  a  coordinate  frame 
attached  to  the  left  elbow.  The  attitude  of  this  link,  0j  2,  is  zero  when  the  link  is  parallel 
with  the  proceeding  link,  Link  LI.  Similar  coordinate  frames  and  definitions  apply  to  the 
right  manipulator.  The  payload  attitude,  Q\\  is  referenced  to  the  inertial  frame.  The  Carte- 
sian coordinates  of  the  payload  center  of  mass  are  also  with  respect  to  the  inertial  frame. 
A  set  of  generalized  coordinates  which  describe  the  system  include  the  centerbody  atti- 
tude, joint  angles  for  all  of  the  manipulator  links,  and  payload  attitude  and  position. 


q  = 


0GeLl  eL29Rl  eR20PXP  YP 


(1) 


Six  joint  actuators  apply  torques  at  the  shoulder,  elbow,  and  wrist  of  each  manipulator. 
A  reaction  wheel  applies  a  torque  to  the  centerbody.  The  actuators  can  be  grouped  into  a 
control  vector 


u  = 


nT 


Uwh  ULS  ULE  ULW  URS  URE  URW 


(2) 


where  the  first  element  is  the  reaction  wheel  torque.  The  remaining  elements  are  joint 
actuator  torques.  The  first  letter  of  their  torque  subscripts  indicates  left  or  right  arm.  The 
second  subscript  indicates  shoulder  (S),  elbow  (E)  or  wrist  (W). 


B.  EQUATIONS  OF  MOTION 

The  equations  of  motion  for  this  system  are  developed  using  Lagrange's  equations  for 
a  dynamic  system  with  holonomic  constraints. 


dt 


dq 


(3) 


subject  to  constraints 


A_q  +  AQ  =  0  (4) 

where  L  =  T  -  V 

T  is  kinetic  energy 

V  is  potential  energy 

q  is  the  generalized  coordinates  vector 

q  is  the  generalized  velocities  vector 

Q  is  the  vector  of  applied  nonconservative  forces 

A  A,  are  the  constraint  forces 
Beginning  with  Lagrange's  equation,  the  equations  of  motion  can  be  rearranged  into 
an  alternate  form.  The  inertia  matrix,  M,  is  a  function  of  the  generalized  coordinates. 
Since  the  potential  energy  is  a  function  of  only  the  generalized  coordinates,  the  partial  of 
the  Lagrangian  with  respect  to  the  generalized  velocities  does  not  contain  any  potential 
energy  terms. 

—   =  Mq  (5) 

dq 


Differentiating  Eq.  (5)  with  respect  to  time  leads  to 


<d 
dt 


'dO 


**•■  TdM  u-  TdM   ■ 

=  Mq  +  q    —  =  Mq  +  q    ~q  (6) 

dt  -     dq  - 


dL 
5q 

1  f :\cM  \ 

-2^TqV- 

av 

"5q 

Replacing  the  Lagrangian  with  expressions  for  kinetic  energy  and  potential  energy 
results  in 
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(7) 


Eq.  (6)  and  Eq.  (7)  can  be  substituted  back  into  Eq.  (3)  to  produce 

M3+2&V)+ar9+A-  (8) 


The  second  term  on  the  left-hand  side  of  Eq.  (8)  contains  the  centripetal  and  Coriolis 
torques.  Replacing  this  with  the  G  matrix  leads  to 

Mq  +  G(q,q)+f^  =  Q  +  ATA.  (9) 

dq 

After  substituting  the  matrix  form  of  the  generalized  forces  into  the  equations  of 
motion,  one  has 

dV  t 

Mq  +  G+ —   =  Bu  +  A   X  (10) 

3q 

The  following  sections  develop  expressions  for  the  inertia  matrix,  Coriolis  and  cen- 
tripetal accelerations,  generalized  forces,  and  constraints  imposed  by  the  closed  chain 
geometry  of  the  system. 

1.    Inertia  Matrix,  M 

The  inertia  matrix  is  found  by  calculating  the  system  kinetic  energy  and  expressing 
it  in  the  form 

T  =  ^qT[M(q)]q  (11) 

The  inertia  matrix  is  the  term  bracketed  by  the  generalized  velocity  vectors.  The 
total  system  kinetic  energy  is  the  sum  of  the  kinetic  energy  of  all  the  pieces. 

T  =  T0  +  TL1+TL2  +  TRI+TR2  +  TP  «2> 

Kinetic  energy  of  individual  components  can  be  found  from 

T    =  -I(o2+-m  (rr)  (13) 

i       2  •    '      2     >  -    - 

I,  is  the  member  moment  of  inertia  about  its  center  of  mass 

co,  is  the  angular  velocity 

m^  is  the  mass 

r  is  the  inertial  velocity  of  the  center  of  mass 
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The  centerbody  angular  rate  and  center  of  mass  position  vector  are  given  by 
Differentiating  Eq.  (15)  results  in  the  velocity  of  the  centerbody  center  of  mass 

r0   =  'cO°V<>  (16) 

Substituting  Eq.  (14)  and  Eq.  (16)  back  into  the  expression  for  kinetic  energy  (Eq. 
(13))  and  collecting  on  the  angular  rate  term  leads  to 

To=  2(I<'  +  m'^>Oo  <17) 

Similar  developments  are  used  for  each  of  the  remaining  pieces  in  the  system.  For 
the  left  manipulator  link  between  the  shoulder  and  elbow  (Link  LI ),  the  angular  velocity  is 
a  combination  of  centerbody  rotation  and  rotation  of  the  link  with  respect  to  the  center- 
body. 

(oL1  =  o0  +  (iL1  (18) 

The  position  vector  to  the  center  of  mass  is 

The  first  two  terms  in  the  position  vector  represent  the  location  of  the  left  shoulder. 
Differentiating  the  position  vector  gives  the  expression  for  the  velocity  vector.  Because 
none  of  the  coordinate  axes  used  in  the  position  vector  expression  are  inertial,  their  rota- 
tion must  be  included  as  well. 

rL,  ='LOroOCOS0LOyoAoWllS1,1\o^+^cL1«1.,yL1  (20) 

After  Eqs.  (18)  and  (20)  are  substituted  into  Eq.  (13)  and  terms  are  grouped  by 
angular  rates,  the  kinetic  energy  is 

TL1  =eo(0.5(IL)+mL141+mL]^0)  (21) 
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+  mLlZLOZcLl^SineLOSin(eLO  +  eLl)+COSeLOCOS(eLO  +  eL|))   > 

+  0.5dJ1(IL,+mLI^L1) 

+  G0HL1(IL1+mL/c2u+mL/1(/cM(sinH|(|sin  ((),„  +  (),,)  +  cos()LOcos  (0,  f)  +  0U  ) ) ) 

The  angular  rate  for  the  left  forearm  includes  the  centerbody  angular  rate  as  well  as 
the  angular  rates  of  the  body  axes  on  Links  LI  and  L2. 

o)L2  =  ()„  +  <),,+(),  2  (22) 

This  link's  center  of  mass  position  vector  is 

rL2  =  ^LOcosG1  (Jx0  +  J^sine,  „>,,  + *,_  ,«u  +  t^x^  (23) 

Differentiating  the  position  vector  gives  the  velocity  vector 

rL2  =  ^00)0COS°LOyoAo0)0Sin0|  .0*0+<U«>Ll?L1  H-L2COL2>u  (24) 

The  kinetic  energy  expression  for  Link  L2  is  found  after  substituting  Eqs.  (22)  and 
(24)  into  Eq.  (13)  and  collecting  terms  with  common  angular  rates. 

TL2  =  0j(O.5(IL2  +  mL242  +  mL2£1+mL2£))  <25) 

+  mL2ZLO£Ll(sinGLOSin(OLO+0Ll)+COSeLOCOS(°LO  +  eLI))+mL2ZL/cL2COsGL2 
+  mL2^o'cL2(sinOLOsin(OLO  +  ni.1+nL2'+COSeLOCOS(OLO+6Ll+0L2>)    * 


.2 


+  G,  i  (0.5  (I,  .  +  m,  -L  ,  +m,  .1  .  J  +  m,  A.  .1  ,  ,cos0,  ,) 

Li  L2  L2  LI  L2  cL2'  L2  LI   cL2  L2 

°-5eL2nL2  +  niL2^L2) 


+ 


+  eoeLi(IL2  +  mL2^i+"1I,42  +  2mL^LI/cL2cos0L2 


+  mL2^0^1(sil,%sin(V  +  0LI)  + COS°L0COS ( 6L0 + BL 1 > > 

+  mL2^cL2(sin0LOsi"'n,o  +  (,,,+UL2)+COs0LOCOS<010  +  GL.+eL2)>   > 

+  ML2  (IL2  +  ^2^2  +  n1L2^Ll^cL2COSeL2 

+  mL2^cL2(sineLOSln(ni.O  +  ,,,.,+,)L2>+COSeLOCOS(%+eL1+eL2))) 
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+  OLiOi.2(IL2  +  m12^2L:+m1^1/cL;cosni2) 

The  development  for  the  right  manipulator  kinetic  energy  parallels  that  for  the  left 
manipulator.  For  the  upper  arm  portion  (Link  Rl ),  the  angular  rate  is 

(oR1  =  e0+eR1  (26) 

The  position  vector  is  constructed  by  finding  the  coordinates  of  the  right  shoulder 
and  adding  the  vector  from  the  shoulder  to  the  center  of  mass. 

'Rl  =*ROCOSeRO*0+Vsin(Wo+'cR1*R)  (27) 

The  time  rate  of  change  of  the  position  vector  is 

rR1  =^Ro0)ncoseRo>'o-^o(0osin0R(»Xo  +  ^R1«R1yR1  (28) 

After  calculating  the  kinetic  energy  for  Link  Rl  and  grouping  terms  with  common 
angular  rates,  the  resulting  expression  is 

TR]  =  M0.5(IR1+mR14I+mR14)  (29) 

+  mRlWcRl(sin0ROSin(ORO  +  OKl)+COSeROCOS(BRO  +  eRl^   > 

+  0.5eR1(IR1+mR14,) 

+  eoeR)(IR1+mR/c2R1+mR/Rf/cR](sinORnsin(0RO+0R1)+coS0ROcos(eRO  +  0R1))) 

Angular  rate  of  the  right  forearm  is 

»R2  =  0o  +  0ri+Gr2  (30) 

Its  position  vector  is 

rR2  =  VoseRO*o  +  VjsilieRoyo+'R1*R1  +'cr:\2  (31) 

Differentiating  Eq.  (31)  produces  the  velocity  vector  for  Link  R2  center  of  mass. 

fR2  =  ZROa>0COSeROyo-^RoC008il,eRO*0+^,»RI>R1  +'cR2«»R2yR1  (32) 
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The  kinetic  energy  resulting  from  substituting  Eqs.  (30)  and  (32)  into  Eq.  (13)  and 
collecting  common  angular  rate  terms  is 

TR,  =  9o<0-5(IR2  +  mR2/cR2  +  mR2/Rl+,nR24o)  (33) 

+  mR2WRI(sin0ROSin(0RO+(,R.)+COS°ROCOS(()RO  +  0R|)»+mR2^/cR2COs0R2 
+  mR2WeR2(sil,0R0sin,nRn+OKl+nR2»   +  COS°ROCOS  (  V  +  6R1  +  (,R2>  >    > 

+  6R,(0.5(IR2  +  mR24,+mR2Z^2)+mR2^^cR2coSeR2) 
+  0.56R2(IR2  +  mR242) 

+  eoGRI(IR:+mR24,+i"R/c2R:+2mR^R/cR2cosOR2 

+  mR2/R«/Rl(sinGR0sin(,)R0  +  0Rl)  +  COS%,COS  (  °RO  +  °R  1  >  > 
+  nlR2VcR2(sin0ROSm<URO+nRl+OR2)+COsOROCOS(ORO  +  eRl+OR2"    > 
+  MR2(IR2  +  mR2*cR2  +  mR2*RI/cR:COS°R2 

+  mR2ZRrAR2(sin()R0Sin((,R0  +  BR.+,,R2>+COs0R0COS(0R0  +  GRl+6R2»)) 

+  eRieR2(IR2  +  mR2<cR2+nlR2'Rl'cR2COs0R2) 

Expressions  for  the  payload  are  considerably  simpler  because  inertial  coordinates 
are  available.  The  angular  rate  is 

»P  =  eP  (34) 

It  is  not  necessary  to  describe  the  payload  center  of  mass  by  passing  through  either 
shoulder  as  was  the  case  with  the  manipulator  links.  The  position  vector  is 

rp  =  Xpflx  +  Ypfly  (35) 

The  velocity  vector  is  also  simpler  because  of  the  inertial  frame. 

rp  =  XPtix  +  YpN,  (36) 
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The  payload  kinetic  energy  is  derived  from  substituting  Eqs.  (14)  and  (16)  into  Eq. 


(13). 


TP=  Hlpep  +  mp<Xp  +  Yp)) 


(37) 


After  substituting  the  expressions  for  kinetic  energy  from  Eqs.  (17),  (21),  (25), 
(29),  (33)  and  (37)  into  Eq.  (12)  and  expressing  the  result  in  the  matrix  form  of  Eq.  (3),  the 
inertia  matrix,  M,  is  given  by  Eq.  (38)  Because  the  generalized  coordinates  for  the  pay- 
load  are  referenced  to  an  inertial  coordinate  frame,  the  inertia  matrix  is  decoupled  between 
the  payload  and  the  rest  of  the  system.  Coupling  does  exist  between  the  spacecraft  center- 
body  and  each  of  the  manipulators. 

Mll  M12M13  M14M15  °     °      ° 


M  = 


M2,  M22 

M23 

0       0     0 

0 

0 

M31  M32 

M33 

0       0     0 

0 

0 

M4,     0 

0 

M44  M45  ° 

0 

0 

M5]     0 

0 

M54  M55  0 

0 

0 

0       0 

0 

0       0     Ip 

0 

0 

0       0 

0 

0       0     0 

mp 

0 

0       0 

0 

0       0     0 

0 

m, 

Expressions  for  the  individual  elements  in  the  inertia  matrix  are  given  by 

M55  =  ^2  +  ^2^2 

M 


'45 


M54  =  M55  +  mR2*R1/cR2coseR2 


M.<  =  1VL.  =  M.,  +  mp^Pf/cp,cos(0R1+ep,) 
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51 


45 


R2  R0  VR2 


R 


R2J 


M44  =  M45  +  lR\+mR2iR\tcR2COsQR2  +  mlUicl\+mR2^\ 


(38) 


(39) 
(40) 
(41) 

(42) 
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M14  =  M41   =  M44+/R0<mR/CRl+mR2^1>COSeR1 
+  mR2WCR2COS(eRl'f0R2> 


M33   =   ^2  +  ^2^2 


M23   =  M32   =  M33  +  mL2^/CL2COs9L2 


M13   =   M31    =  M23+mL2WCL2COS^Ll+0L2) 


(43) 


(44) 
(45) 
(46) 


M22  =  M23  +  lL\+mL2lUicL2QOsQL2  +  mUicl\+mL2lll  <47> 


M12   =   M21    =  M22+^L0^mLl^CI.l  +mL2^Ll^C0Sei,l 
+  mL2WCL2COS(eLl+0L2> 


(48) 


Mll    =   I0  +  M22  +  M44  +  m(/C(2»+^mi.l+mL2^2L0+  (mRi+mR2^2RO  (49> 

+  2/R0(mR]/cR1+mR2/R])coseR1+2mR2/R0/cR2cos(eR1+eR2) 

+  2Vo  (mLl/CL  1  +  m\Jl.  1 )  C0SeL  1  +  2mL2  WCL2C0S  <9L  1  +  9L2> 
2.    Centripetal  and  Coriolis  Matrix,  G 

The  G  matrix  contains  all  of  the  centripetal  and  Coriolis  terms.    It  is  most  easily 
found  using 


G(q,  q) 


qV>g 
qV2>q 


■T     (8)  . 
q    C       q 


(50) 


where  the  elements  of  C^1-*  are  defined  by  the  Christoffel  symbol 

Mi)        1 


'jk 


(dU-     5M..      DMA 
i.l  ik  jk 


dq. 


dq 


c\ 


(51) 
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The  form  of  the  G  matrix  for  the  system  of  Figure  1  is  given  as 


G  = 


Gl 
G, 


Gi  =  ~*lo(0li  +20O9L1)  (mu*cL,+mL2*L1)siii9L| 
-mL2ZLi'cL26L2(2^o  +  2eL1  +  GL2)  sin0L2 

-mL2/LOZcL2(26O^Ll+eL2)  +  «>1.  1  +  ^L3>  ^~  >  si"  '  \  |  +  0L2> 

.2  .     . 

Ao(eRl+2eO0Rl)   (mR/CRl+mR2^Rl)Sm9Rl 

-mR2^/cR2eR2(2e0  +  2oRI  +eR2)  sin0R2 

-mR2^cR2(29o(ORi+eR2)  +(0R,+0R2)  )sin(9R1+0R2) 

.2  .... 

G2   =  'LO0'J(nlLl'CLl+mL2'Ll)sinOLl~mL2^I  l/cL2°L2(2e0  +  20Ll+GL2)sineL: 

+  mL2'LoZcL26osin(eL1+GL2) 
G3  =  miA/cL2<9Ll+6L2)   sin0L2  +  mL2/LOZcL20osin(GL1+eL2) 

.2  .... 

G4  =  ^oeO<mR/CR|+mR2^Rl)sll,eRl~mR2^Rl/cR2BR2(20O  +  20Rl+eR2)slne 

+  »1R2<Ro'CR20Osin(0R1+0R2> 

2  .2 

G5   =   mR2^Rl^CR2(0Rl+0R2)    S,neR2  +  mR2WCR29Osln(0Rl+0R2) 


R2 


(52) 


(53) 


(54) 


(55) 
(56) 


(57) 


18 


3.    Generalized  Forces,  Q 

The  generalized  forces  are  found  using  the  principle  of  virtual  work.  When  there 
is  no  reaction  wheel  on  the  centerbody,  the  system  does  not  experience  any  external  forces 
capable  of  performing  work.  Six  joint  actuators  apply  torques  at  the  shoulder,  elbow,  and 
wrist  of  each  manipulator. 


U6  = 


ULS  ULE  ULW  URS  URE  URW 


(58) 

uc  is  simply  the  joint  actuator  subset  of  the  complete  actuator  torque  vector,  u.  The 
total  virtual  work  is  the  sum  of  the  torques  applied  to  the  individual  bodies  times  their  vir- 
tual angular  displacements. 

N  N 

5W  =    £  5W,  =    X  (M,)60,  <59> 

i  -  1  i  =  1 

When  the  left  shoulder  joint  actuator  applies  a  positive  torque  on  Link  LI,  a  nega- 
tive torque  is  also  applied  to  the  centerbody.  The  virtual  work  performed  by  the  left  shoul- 
der motor  is 

5WLS  =  ULS<50(»  +  5eLl-6(V  <60> 

where  the  positive  angles  are  those  associated  with  the  change  in  Link  LI  attitude  and  the 
negative  angle  is  associated  with  the  change  in  centerbody  attitude.  The  left  elbow  actua- 
tor makes  a  positive  contribution  to  Link  L2  attitude  and  a  negative  contribution  to  Link 
LI  attitude. 

5WLE  =  uLE(8e0  +  6eLI+5ei.2-5e«-5ei.|)    =  ULE59L2  <61> 

The  joint  actuator  at  the  left  wrist  makes  a  positive  change  in  the  payload  attitude 
and  a  negative  change  in  Link  L2. 

5WLW  =  uLW  (80p  -  60o  -  59L1  -  59L2)  (62) 
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The  right  shoulder  actuator  makes  a  positive  contribution  to  Link  Rl  attitude  and  a 
negative  contribution  to  centerbody  attitude. 

5WRS  =  uRS(89()  +  6eR|  -50o)  =  uRS89R1  (63) 

Link  R2  has  a  positive  virtual  displacement  due  to  a  positive  torque  at  the  right 
elbow.  The  same  torque  causes  a  negative  virtual  displacement  of  Link  Rl . 

5WRE  =  URE  <660  +  59R 1  +  86R2  "  86«  -  86R I  >    =  URE5eR2  <64> 

The  right  wrist  actuator  has  a  positive  influence  on  the  payload  and  a  negative 
influence  on  Link  R2. 


5W, 


(65) 


RW  =  URW<50p-5e(.-59Rl-5eR2) 
Gathering  Eqs.  (60)-(65)  together  produces 

8W  =   (-  uLW  -  uRW)  50o  +  (uLg  -  uLW)  59L  {  +  (uLE  -  uLW)  50L2        (66) 

+  (URS-URw)5eRl+  ^URi;-URW>59R2+  (ULW-URw)59P 
With  respect  to  the  system  equations  of  motion,  the  generalized  force  correspond- 
ing to  a  particular  generalized  coordinate  is  that  portion  of  the  virtual  work  associated  with 
the  same  generalized  coordinate.  Now  Eq.  (66)  can  be  transformed  into  a  matrix  form. 

%  =  B6U6  <67> 

where  B  is  the  control  influence  matrix  given  by 


B6  = 


00 

-10  0 

-1 

1  0 

-10  0 

0 

0  1 

-10  0 

0 

0  0 

0    1  0 

-1 

00 

0   0  1 

-1 

00 

1    00 

1 

00 

0   00 

0 

0  0 

0   00 

0 

(68) 
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The  only  effect  of  a  positive  reaction  wheel  torque  applied  to  the  spacecraft  is  to 
alter  the  centerbody  attitude  in  a  positive  direction.  This  manifests  itself  in  the  control 
influence  matrix  in  the  form  of  another  column.  This  new  column  is  all  zeros  except  for  a 
single  one  corresponding  to  the  location  of  the  reaction  wheel  torque  in  the  u  vector.  With 
the  reaction  wheel  torque  as  the  first  element  in  the  control  vector  as  in  Eq.  (2),  the  com- 
plete control  influence  matrix  is 


10  0-100-1 
0  10-100  0 
00  1-100  0 
0  0  0  0  10-1 
0  0  0  0  0  1-1 
0  0  0  1  0  0  1 
00  0  0  00  0 
00  0  0  00  0 


B  - 


(69) 


4.    Constraints  Matrix,  A 

Because  the  eighth  order  system  under  consideration  has  only  four  degrees  of  free- 
dom, an  additional  four  equations  are  needed  to  describe  the  constraints.  The  eight  gener- 
alized coordinates  are  not  independent.  The  constraint  equations  embody  the  information 
that  the  manipulators  are  both  grasping  the  payload  forming  a  closed  chain  system.  The 
constraints  matrix  is  derived  by  writing  the  system  constraints  in  the  Pfaffian  form  as 

Aq  +  AQ  =  0  (70) 

These  equations  come  from  geometric  relationships  of  expressing  the  payload  cen- 
ter of  mass  Cartesian  coordinates  in  terms  of  the  other  generalized  coordinates. 


/L0cos(60  +  9L0)  +*L1cos(90  +  eL0  +  0L1)  +*L2cos(Oo  +  0LO  +  eLl  +0i.2)  +  &pcos9p  =  xp 

^0»n(e0+eL0)+/L1«n(e0+9L0+eLI)+/L2Bin(e0+eL0+eLI+eL2)+fcp8inep  =  yp 

^ROcos(eo  +  eRO)+^lCos(Go+GRO  +  eR1)+^R2cos(0o  +  eRO  +  GR1+eR:)-(^-iCp)cosGp  =  xp 

^RoSin(Go  +  GRO)+<R1sin(Go  +  GRO  +  0R1)+^R2sin(Oo  +  OR(|  +  OR1+GR2)-(^-^cp)sinGp  =  yp 


(71) 
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To  get  the  Pfaffian  form  of  Eq.  (70),  differentiate  Eq.  (71)  and  rearrange  terms. 
The  result  is 


AMAI2AM  0  0  A|6-l  0 
A2,  A22  A^3  0  0  A2(.  0  -1 
A3,     0      0    A34A35  A,6  -I    0 

A41     0      0    A^A^A^   0    -1 


'ii 


L2 


(I 
«R2 

eP 

XP 


(72) 


The  constant  term,  Aq,  is  a  zero  vector.  The  individual  element  in  the  constraints  matrix 
are  given  by  the  following  equations 

(73) 


A16  =  -*cpsin0P 


A26  =  ^cpcos0p 

A36  =    (^p^cp)  sin0P 

A46  =  '  ^p~^cp)  cos0p 

A45  =^R2cos(0o  +  0RO  +  0R1+eR2) 

A44  =   A45  +  VlCOS(e(»  +  eR0  +  9Rl) 


A41    =   A44+^()COS(60  +  eR0) 


A35  =  ^R2sin(0U  +  0RO  +  0Rl+eR2> 
A34  =  A35-^Rlsin(e()  +  0RO  +  eRl) 


A31    =  A34-*ROsin<e0  +  eR0> 


A23  =  ^2cos(0o  +  0lo  +  0li+0L2) 


(74) 

(75) 
(76) 
(77) 
(78) 
(79) 

(80) 
(81) 
(82) 
(83) 
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A22   =   A23  +  L\.  1  C0S  (9U  +  °L()  +  9L  1 )  <84) 

A21    =   A22  +  ^I()COS^e(»  +  0l.(»)  <85> 

A13  =  -^2sin(e0  +  eL0  +  GL1+eL2)  (86) 

A12   =   A13-^lsin(0(»  +  ei.(»  +  0l.l>  (87> 

All    =A12^LOsin(0O  +  0Lo)  <88> 

If  the  manipulators  are  mounted  on  a  fixed  platform  rather  than  a  rotating  base,  an 
additional  constraint  equation  is  included  in  the  A  matrix.  The  constraint  is  that  0()  is  con- 
stant and  therefore 

e0  =  o  (89) 

This  constraint  is  augmented  into  the  A  matrix  by  adding  a  fifth  row.  The  first  ele- 
ment in  the  row  is  a  one.  The  remaining  seven  elements  are  all  zeros. 

C.  SIMPLIFIED  EQUATIONS  OF  MOTION 

The  potential  energy  term  is  zero  because  motion  is  confined  to  the  horizontal  plane 
and  the  system  is  composed  of  rigid  members  The  inertia  matrix,  G  matrix,  B  matrix,  and 
constraints  matrix  can  be  found  from  the  results  of  the  previous  sections.  The  remaining 
unknowns  are  the  actuator  torques  and  the  Lagrange  multipliers.  By  using  the  equations 
of  motion  and  the  Pfaffian  form  of  the  constraints,  one  can  eliminate  the  Lagrange  multi- 
pliers. The  time  derivative  of  Eq.  (70)  is 

Acj  +  Aq  =  0  (90) 

Solving  Eq.  (10)  for  q  and  substituting  the  result  into  Eq  (90)  permits  one  to  find  an 
expression  for  the  Lagrange  multipliers. 
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(AM  V7)      (AM   1(G-Bu)-Aq) 


(91) 


The  inertia  matrix  is  always  a  square  matrix  with  full  rank  and  therefore  invertible.  To 

-IT 
investigate  the  invertiblity  of  AM     A   ,  begin  by  creating  a  4x4  matrix  out  of  the  third, 

fifth,  seventh,  and  eighth  columns  of  the  constraints  matrix. 


L13 

0 

-1 

0 

L23 

0 

0 

-1 

0 

A35 

-1 

0 

0 

A45 

0 

-1 

(92) 


Inspection  of  this  submatrix  reveals  that  all  of  the  rows  and  columns  are  linearly  inde- 
pendent even  if  A]3  =  A23  and  A35  =  A45.  Therefore,  the  A  matrix  always  has  rank  of  4. 

-l    T 
The  4x4  matrix  product  AM     A    will  also  always  have  rank  of  4  and  is  therefore  invert- 
ible. Eq.  (91)  can  be  substituted  back  into  the  equations  of  motion  (Eq.  (10))  leaving  the 
actuator  torques  as  the  only  unknowns.    The  resulting  equations  of  motion  in  which  the 
Lagrange  multipliers  have  been  removed  and  potential  energy  is  zero  are 


where 


Mcj  +  G  =  Bu 


G  =  G-A1  (AM   ]AF)      (AM   !G-Aq) 


B  =     II-AT(AM  V)     AM"' JB 


(93) 


(94) 
(95) 


D.  REFERENCE  TORQUES 

Given  a  reference  trajectory  of  the  payload  with  known  displacements,  velocities  and 
accelerations,  one  can  use  the  simplified  equations  of  motion  (Eq.  (93))  to  solve  for  the 
actuator  torques  that  will  produce  the  reference  trajectory. 
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M    A     -+G,,.r  =  BrH-u    r  (96) 

ref  Vci  lcl  ,cl    rci 

The  equations  for  specific  elements  in  the  matrices  of  Eq.  (96)  are  the  same  as  already 
presented.  The  subscript  "ref  merely  means  that  the  displacement,  velocity  and  accelera- 
tion terms  are  the  values  from  the  reference  trajectory 

In  this  study,  the  total  number  of  actuators  is  more  than  the  system  degrees  of  freedom. 
This  situation  is  caused  by  the  geometric  constraints  of  multiple  manipulators  handling  a 
common  object.  As  a  result,  there  are  an  infinity  of  solutions  for  the  reference  torques. 
One  method  to  select  a  soecific  solution  is  to  establish  a  cost  function.  An  obvious  cost 
function  is  to  minimize  a  weighted  norm  of  the  actuator  torques. 

J  =  -UT  W  u    e  (97) 

2  ~ref     u   ref  y      ' 

The  problem  now  becomes  one  of  minimizing  the  cost  function  (Eq.  (97))  subject  to 
the  constraint  that  the  reference  equations  of  motion  (Eq.  (96))  are  satisfied.  Augmenting 
the  cost  function  with  the  constraint  by  means  of  another  Lagrange  multiplier  leads  to 

J  =  1UTW  u      +YT(Brefu     -M    rq    r-G„f]  (98) 

2 -ref     u-ref      '     V    Iel-ref        ref^ref         ,eV  y      ' 

The  minimum  of  the  augmented  cost  function  is  found  by  taking  the  gradient  of  Eq. 

(98)  with  respect  to  the  reference  torques  and  with  respect  to  the  Lagrange  multiplier. 

Each  of  the  gradients  is  set  to  zero. 

~T 


Vu    J  =  0  =  W  u      +B,y 
uref  u   rel        rel-' 


(99) 


VrT  =  0  =  Brefuref-Mrefqref-Gref  (100) 

Eqs.  (99)  and  (100)  are  two  equations  in  two  unknowns  (y,  u     ).    Eliminating  y 
results  in  an  expression  for  the  reference  actuator  torques. 
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»rer  =  W^LflXrW-'O    (Mrefqrcf+Gref)  (101) 


■T 


Although  the  matrix  product  Brefwu'Bref  is  an  8x8  matrix,  it  is  not  invertible.  A 
pseudo-inverse  is  needed  because  the  system  has  only  four  degrees  of  freedom.  There- 
fore,  the  matrix  product  Brefw~'Bref  is  rank  deficient  and  has  a  rank  of  four  at  most.  This 
expression  for  reference  actuator  torques  minimizes  the  augmented  cost  function  (Eq. 
(98))  at  each  instant  in  time.  Although  the  value  for  the  reaction  wheel  torque  is  calcu- 
lated, it  is  not  minimized  by  this  function.  The  reaction  wheel  torque  profile  is  dictated  by 
the  disturbance  torques  transmitted  to  the  centerbody  as  a  result  of  manipulator  and  pay- 
load  motion.  For  a  given  reference  trajectory,  an  infinite  variety  of  joint  actuator  torques 
can  produce  that  trajectory.  However,  a  given  reference  trajectory  has  only  one  reaction 
wheel  torque  profile  that  is  common  to  all  the  infinity  of  joint  torque  combinations  associ- 
ated with  that  trajectory.  Equation  (101)  selects  from  among  the  infinity  of  joint  actuator 
torques  the  one  combination  that  minimizes  the  weighted  norm  cost  function.  Although 
the  selection  is  limited  to  a  single  choice,  Equation  (101)  also  produces  the  correct  reac- 
tion wheel  torque  for  the  given  reference  trajectory. 

E.   LYAPUNOV  CONTROLLER 

This  material  in  this  section  is  based  on  Ref.  16.  The  purpose  of  any  control  law  is  to 
provide  system  performance  that  satisfies  a  specification.  As  a  bare  minimum,  the  control 
law  must  keep  the  system  stable.  Because  of  the  highly  nonlinear  nature  of  this  spacecraft 
robotics  system,  most  control  laws  simply  do  not  apply  The  motivation  behind  using 
Lyapunov  methods  is  to  develop  a  control  law  with  guaranteed  stability.  Recall  the  equa- 
tions of  motion  of  the  manipulator  system  are 

Mq  +  G  =  Bu  (102) 
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Solving  Eq.  (102)  for  q  results  in 

q  -  M"1  (Bu-G)  (103) 

Substituting  Eqs.  (94)  and  (95)  back  into  Eq   (103)  and  grouping  terms  according  to 
the  form 

q  =  CjU  +  C2q  +  C3  (104) 

leads  to  the  following  expressions 

C,  =  M"1  {I-A1  (AJvT'a1)     AIVf'jB  (105) 

C2  =  -IVT'A^AIVf'A1)     A  (106) 

C3  =  M-1  {AT(AM_IAI)     AM~'-I}G  (107) 

Similarly,  the  reference  maneuver  accelerations  can  be  expressed  as 

q       =  C.     u      +C,    q    p+C.  (108) 

-ref  lrer'ci         2reiref         3ref 

where  again  the  reference  subscripts  on  the  C  matrices  indicate  that  reference  maneuver 
values  need  to  be  used  in  their  calculation.  Let  error  quantities  between  the  actual  vari- 
ables and  their  reference  maneuver  counterparts  be  defined  by 

Sq  =  q-qref  (109) 

5q  =  q-qref  (110) 

5q  =  q-qref  (111) 

Now  define  an  error  Lyapunov  function  as 

U  =  0.5(5q-5q)  +f(5q)  (112) 

where  f(6q)  >  0.  Differentiating  Eq.  (112)  results  in 
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0  =  5q-6q  +  £ 


Pf 


-^(oq,) 


H 


(113) 


Let 


F  = 


af        er 


dt 


a(5q,)    3(5q2)         c1(5q7) 
Then  Eq  (114)  can  be  rewritten  as 


(114) 


U  =  5q-  (5q  +  F)  (115) 

Substituting  Eq.  (104)  and  Eq.  (108)  into  Eq  (111)  and  then  Eq.  (Ill)  into  Eq.  (115) 
produces 

U  =  5q{(Cu-C      u     )  +  (Cq-C2    q     )  +  (C^  -  C      )+F}        (116) 

ref-  '  *'•'  ref    '  ^'  ref 

If  one  lets  the  quantity  inside  the  brackets  of  Eq  (116)  equal  -K  5q  where  K  is  a 
positive  definite  matrix,  then  one  is  guaranteed  that  U  <  0  and  therefore  the  system  will  be 
stable  in  the  Lyapunov  sense.  Solving  Eq  (116)  for  command  torques,  u,  leads  to 

u  =  C1t{-Kv5q  +  C]    u      -(C2q-C2    q     )  -  (C,  -  C3    )-F}        (117) 

Cj  is  an  8x7  matrix  so  C. '  is  its  pseudo  inverse.  Equation  (1 19)  finds  the  torques  that 
should  be  used  rather  than  the  reference  torques.  All  that  remains  is  to  choose  a  function 
for  f  (5q) .  One  can  choose 


f(5q)  =  ^8qTKp5q 


(118) 


where  like  K  r,  K  is  required  to  be  positive  definite.  Selection  of  values  for  the  gain 
matrices  is  beyond  the  scope  of  this  work.  The  simulations  included  in  the  next  chapter 
use  diagonal  matrices  with  uniform  values  simply  as  a  matter  of  convenience.  One  might 
try  to  adapt  the  linear  quadratic  regulator  (LQR)  problem  to  find  more  optimal  gains. 
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After  substituting  Eq.  (118)  into  Eq.  (114)  and  that  result  into  Eq.  (119),  one  obtains  the 
final  form  of  the  Lyapunov  controller. 

u  =  C1t{-Kv5q  +  C,     u      -(C2q-C2    q     )-(C,-C3    )-Kp5q}     (119) 

1  V  'rerrel  Z  Zref    rel  Jref  ' 

If  the  differences  between  the  reference  trajectory  and  the  system  dynamics  are  small, 
the  Lyapunov  controller  approaches  the  form  of  a  proportional  plus  derivative  (PD)  con- 
trol law. 

F.    REFERENCE  TRAJECTORIES 

The  reference  trajectories  describe  the  nominal  path  that  the  system  follows  in  moving 
from  the  initial  conditions  to  the  desired  final  conditions.  One  need  only  specify  reference 
trajectories  for  as  many  generalized  coordinates  as  there  are  degrees  of  freedom.  In  effect, 
the  generalized  coordinates  can  be  divided  into  two  sets.  One  set  contains  the  minimum 
number  of  coordinates  needed  to  completely  describe  the  system.  The  second  set  contains 
all  remaining  coordinates,  (redundant  coordinates).  The  choice  of  which  generalized 
coordinates  to  specify  is  entirely  arbitrary.  A  reasonable  choice  includes  the  payload  coor- 
dinates and  centerbody  attitude  since  the  user  will  probably  be  especially  interested  in 
these  generalized  coordinates.  The  redundant  coordinates  are  the  four  manipulator  joint 
angles.  Given  reference  trajectories  for  the  minimum  number  of  coordinates  exist,  the 
redundant  generalized  coordinates  can  be  derived.  This  research  assumes  trajectories  are 
available  which  define  displacement,  velocity  and  acceleration  for  the  centerbody  attitude, 
payload  attitude,  and  payload  center  of  mass  coordinates  (Xp  Yp,  9p  and  90). 

1.    Calculating  Redundant  Coordinates 

Figure  2  illustrates  the  relevant  geometrical  relationships  to  find  the  joint  angles  of 
the  left  manipulator.  Xp,  Yp,  0P  and  0()  are  obtained  from  the  reference  trajectory  Point 
LS  is  the  left  shoulder  joint.  It  has  Cartesian  coordinates  given  by 
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Figure  2:  Deriving  Left  Manipulator  Joint  Angles 


LSx  =  <L(]cos(0(i+010>  (120) 

Lsy  =  Vin(00+eLO)  (121) 

Point  Q  is  the  joint  between  the  manipulator  end  and  the  payload.  The  Cartesian 
coordinates  of  this  point  are 

Qx  =  Xp-*cpcos9p  (122) 


Q    =  YD-Z  DsinGD 

^y  P        cP  P 


(123) 
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The  distance  between  the  left  shoulder  and  Point  Q  is  given  by 


'(Qv-LSxr+(Qy-LSv) 


lsq  =  J( 
The  inertial  angle  formed  by  the  vector  from  LS  to  Q  is 

ru.  -'-V 

P    =  atan 


(124) 


(125) 

The  dimensions  of  the  triangle  formed  by  the  manipulator  joints  are  known.  Using 
the  law  of  cosines,  the  interior  angles  at  the  shoulder  and  elbow  can  be  found  from 


P7  =  acos 


p,  =  acos 


I       2*L|LSQ 

'  2<M<,.2 


(126) 


(127) 


All  that  remains  is  to  algebraically  construct  the  manipulator  joint  angles  from 
other  angles  as  follows 

eL1  =  P,+P2-(VeL0)  (128> 

eL2  =  p,  +  180°  (129) 

The  development  for  the  right  manipulator  is  similar    Its  geometry  is  depicted  in 
Figure  3. 

Point  RS  is  the  right  shoulder  joint  with  Cartesian  coordinate 

(130) 

(131) 

Point  P  is  the  joint  between  the  manipulator  end  and  the  payload.   The  Cartesian 
coordinates  of  this  point  are 

(132) 

(133) 

The  distance  between  the  right  shoulder  and  Point  P  is  given  by 


RSx  =  *ROCOS(°O+0RO> 


RSy  =  'Rosi»<eo  +  fW 


px  =  xp+  (/p-*cP)cosep 


V  Yp+i/p-^sinO, 
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Figure  3:  Deriving  Right  Manipulator  Joint  Angles 


RSP  =  ,/(P   -RS  )2+  (P   -RS  )2 
*v       x  x  y  y 

The  inertial  angle  formed  by  the  vector  from  RS  to  P  is 

P4  -  atan 


T   -RS 

y         y 


P   -RS 


(134) 


(135) 


From  the  law  of  cosines,  the  interior  angles  at  the  shoulder  and  elbow  are 


p,  =  acos 


*R1+RSP-42 
2<R,RSP  " 


(136) 
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P,  =  acos 


2<R!<R2 


(137) 


The  geometry  in  Figure  3  gives  the  manipulator  joint  angles  based  on  the  other 
angles. 

(>R,  =  IV1V  «>„  +  %,)  (»38) 

eR2=  i80«-p6  (139) 

Recall  from  the  discussion  of  the  Lyapunov  controller  that  torques  are  calculated 
based  not  only  on  the  generalized  coordinates  but  their  velocities  and  accelerations  as 
well.  The  redundant  coordinates  have  just  been  found,  but  the  redundant  coordinates 
velocities  and  accelerations  must  still  be  developed. 

Differentiating  Eqs.  (122)  and  (123)  expresses  the  velocity  of  Point  Q. 


Qx  =  XP  +  ep/cpsin0p 


(140) 


Qy    =    Yp-VcpCOs0p  (141) 

But  the  coordinates  of  Point  Q  can  also  be  expressed  in  terms  of  left  manipulator 
variables. 

Qx   =   /L0COS(G0  +  eL0)+^.COS(00  +  f)L0+0lV+^2COS(()0+0L0+0L1+GL2)  (l42) 

Qy   =  <L0si,l(e0+eL0>  +*Llsi"(0O  +  eLO  +  eLl)  +  <I.2sil1  (00  +  9L0  +  °L  1  +  6L2>  (143) 

Differentiate  these  equations  and  rearrange  the  terms  in  the  form  of 


l9*j 


D^n  +  D, 


'LI 


'i  : 


(144) 


where 


d2(i,d  =  -^2sin(e0+eLO+e11+o12)-/L1sin(e0+eLO+eL1) 
D2(i,2)  =-^2sin(eo+0LO+eL1+eL2) 

D2(2,l)    =  ^2COS(eo+0LO  +  9Ll+eL2^  +/LlCOSfe0+9L0+GLl) 


(145) 
(146) 
(147) 
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«L1 

D-' 

" 

D,rttl 

1 

0, 

Dj  »,  2)      t{  .cosjo,,  »  ii,  n  nin  i  B,    )  (US) 

0,(1,1)        eLa«in(90  +  0LO  +  eLl+0La)     «L,dn(00  +  BL0  +  8L,)     «,  „m.mii,  ui,  „»  (149) 

l\tl.:i      /,  .i-osjn,,  i0M1  >o,  ,  1-0,  .)  I  C,  ,ooa(0n  h0L0  +  0,  ,)  i  £,  0oo8(90  ut,  „)  (150) 

I  efl  manipulatoi  joinl  velocities  are  round  by  rearranging  Equation  (144) 

(151) 

where  Eqs  (140)  and  (141)  provide  the  expressions  foi  qs  andcx  respectively 

Using  the  same  approach  to  find  the  joinl  velocities  of  the  righi  manipulator,  Point 
P  is  expressed  .>s  .i  function  of  right  manipulatoi  variables 

r-j     ^iin(e0+eR0)  ^wntVWW  »',,-",/,,„"1,;,  ►«Mi  (153> 

Differentiate  those  equations  and  rearrange  tlu*  terms  in  the  form  of 


>\ 


\\0„>\\ 


*m 


(154) 


whoie 


D4(U2)        <k.m.ui>,  m.Ki>  m>k.  |.eRa) 
D     •  n      Jrj«ks(00+0R0+0b    K0Ri)  ^,008(0,  ►« 


D4(2,2)       ^  0081  I    M)H    K0R    I 


(155) 
(156) 
(157) 
(158) 


D,an      «Rj»M»0*eR0+S  ►•M>   ^^n^+e,      ,     —  (159) 

iy,i.-      <K;^i00*0R,  ^    ^     -,  —  >',  ►•,    <<\        ■     oos   I      8R0)  (160) 

Right  manipulatoi  joint  velocities  are  found  b\  rearranging  Equation  (154) 
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_        _ 

f 

> 

Or, 

=  < 

K 

-  D3d0 

8R2 

\ 

A 

; 

(161) 


where  expressions  for  px  and  py  are  found  by  differentiating  Eqs.  (132)  and  (133). 

Px  =  Xp-ePUp-ZcP)sinGp 


(162) 


Py  =  Yp  +  6p(^-«cp)cosOp  (163) 

Manipulator  joint  accelerations  are  found  by  differentiating  the  expressions  for 
velocity  (Eqs.  (144)  and  (154)). 


=  D|0O  +  D.6O  +  D2 


=  D390  +  DA  +  D4 


0L1 

j»L2 

«'r: 


+  D. 


+  D, 


"i.i 
9L2 

Sri 

8R2 


(164) 


(165) 


Solving  for  joint  acceleration  gives 


eL2 


=  d: 


- 

/ 

-    — 

6ri 

■v? 

Px 

_6R2 

V 

A 

0,0,-0,0,-02 


-  D3eo-D,0o-D4 


0L2 


'K  I 


FR2 


(166) 


(167) 


where  the  accelerations  of  Points  Q  and  P  come  from  differentiating  Eqs.  (140)-(  141)  and 
Eqs.  (162)-(1 63).  Derivatives  of  the  D  matrices  are  constructed  by  differentiating  Eqs. 
(145)-(150)andEqs.  (155)-(160). 

2.    Selecting  Reference  Trajectories 

Any  path  which  connects  the  associated  endpoints  can  be  a  reference  trajectory. 
To  help  ensure  that  the  spacecraft  and  payload  do  not  experience  any  unnecessary  jerk  or 
excitation  of  flexible  structures,  one  might  further  constrain  the  path  such  that  the  veloci- 
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ties  and  accelerations  are  zero  at  the  endpoints  Because  a  reaction  wheel  is  required  to 
maintain  spacecraft  attitude,  the  reaction  wheel  torque  history  is  a  prime  candidate  for 
optimization.  Possible  performance  indices  include  the  integral  of  the  absolute  value  of 
reaction  wheel  torque 

J  =   flu   ,     ,|dt  (168) 

J  |    whecll  v        ' 

K 
or  the  maximum  reaction  wheel  torque. 

J  =  max(|u   ,     .1)  (169) 

v|    wheelr 

A  rigorous  method  for  reference  trajectory  selection  is  to  develop  an  optimal  con- 
trol solution  to  the  two  point  boundary  value  problem.  The  performance  index  in  the  opti- 
mal control  problem  is  given  by 

J  =jL[x(i),„(t),t]dt  (170) 

Using  Eq.  (168)  as  an  example, 

L  =  |uwh|  =  |Du|  (171) 

where 

lU  =   [Uwh  ULS   ULE   UI  W  llRS   URF,  URVv)    J  ^172) 

and 

D  =  [l  o  o  ooooj  (173) 

The  state  equations  must  be  formulated  as  first  order  differential  equations  as 

x-f[x(t),u(t),t|  (!74) 

Because  the  system  dynamics  of  my  problem  are  second  order  differential  equa- 
tions, the  state  vector  for  the  trajectory  optimization  is  a  combination  of  generalized  dis- 
placements and  velocities. 
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T      T 


(175) 


The  resulting  state  equations  are 


8x8   8x8 

8x8   8x8 


X  + 


8x7 


M^B 


u  + 


8x1 


-M"'(i 


(176) 


where  G  and  i  are  the  same  matrices  as  already  found  in  Eqs  (94)  and  (95)  respectively. 

Desirable  boundary  conditions  are  such  that  the  payload  is  at  rest  with  zero  accel- 
eration at  the  beginning  and  end  of  the  repositioning  maneuver.  However  because  the 
state  vector  does  not  contain  accelerations,  they  cannot  be  specified  as  a  boundary  condi- 
tions. If  the  state  vector  is  increased  to  include  accelerations,  then  the  first  order  state 
equations  involve  third  order  derivatives  of  the  equations  of  motion  rather  than  second 
order  equations.  This  prevents  including  payload  accelerations  as  part  of  the  boundary 
conditions.  To  permit  further  development  of  the  optimal  control  problem,  the  boundary 
conditions  will  be  limited  to  desired  positions  and  zero  velocity. 


X(V    =    h"n»    °lx«] 

X(V  -  [q'V  °.x8]T 


(177) 


(178) 


The  Hamiltonian  formed  by  combining  the  performance  index  with  the  state  equa- 
tions is 

H[x(t),u(t),Mt),t]  =  L[x(t),u(t),t]  +x  (OfjxCO.uOJ.tJ  (179) 

The  performance  index  and  the  state  equations  are  both  linear  with  respect  to  the 
control  vector,  u.  The  consequences  of  this  are  that  one  cannot  find  a  minimum  by  taking 
the  gradient  with  respect  to  u  and  setting  it  equal  to  zero.  The  applicable  control  form  is 
bang-bang.  Separating  the  Hamiltonian  into  those  terms  which  premultiply  u  and  those 
which  do  not  leads  to  the  control  law 
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u*   =  u^sigiKH,)  (180) 

where 

H  =  H0  +  H,u  (181) 

The  other  equations  which  must  be  satisfied  are 

i1.."!.-*-!**  (,82) 

dx  dx  dx 

Because  the  performance  index  is  only  a  function  of  u,  the  first  portion  of  the 
above  necessary  condition  is  trivial. 

dh 


dx 


(183) 


3f 

is  not  as  easily  found.  The  M,  G,  A,  and  a  matrices  are  all  functions  of  the  state 

dx  J 

vector.  In  addition,  the  complexity  is  increased  by  several  matrix  inversions  in  the  expres- 
sion of  f  in  the  6  and  b  matrices.  Although  an  analytical  expression  may  be  theoretically 
possible,  finding  it  was  found  to  be  extraordinarily  tedious. 

Recall,  however,  that  the  usefulness  of  the  reference  trajectories  is  to  specify  the 
generalized  coordinates,  velocities,  and  accelerations.  Therefore,  a  convenient  form  for 
the  reference  trajectory  is  as  a  polynomial  function  of  time.  The  following  development 
uses  the  payload  attitude  generalized  coordinate  to  illustrate  how  the  polynomial  reference 
trajectories  are  applied.  Let 

A9p  =  ep(tf)-ep(t0)  (184) 

where  t^  is  the  maneuver  start  time  and  tf  is  the  final  time.  The  duration  of  the  maneuver 
is  the  difference  between  tf  and  \q.  Gp(tfj)  and  Gp(tj-)  are  the  initial  payload  attitude  and  the 
desired  final  attitude  respectively.  If  the  desired  reference  path  for  the  payload  attitude  in 
moving  from  initial  to  final  conditions  is  a  curve  which  can  be  represented  as  a  polyno- 
mial function,  f(x),  where  x  is  simply  normalized  time 
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t  =  —  (185) 

then 

ep    (t)  =  8p(t  )  +f{T)  (A9p)  (186) 

ref 

9P    (t)   =  f  (t)  (A9  )[— !—  (187) 


'^-'o 


dp    (t)  =  r  (t)  ( A9)  I — - I  (188) 


'  tv-v 


In  order  for  Eq.  (186)  to  produce  the  correct  initial  and  final  values  for  o     ,  the 

r.I 

polynomial  must  be  such  that 

f(i=0)  =  0  (189) 

f(T=l)=l  (190) 

To  produce  zero  velocity  and  acceleration  at  the  initial  and  final  conditions 

requires  that  f(x)  also  satisfy 

f(x=0)  =  0  (191) 

f'(x=l)  =  0  (192) 

f'(T=0)  =  0  (193) 

f'(T=l)  =  0  (194) 

The  minimum  order  polynomial  which  satisfies  the  boundary  conditions  of  Eqs. 
(189)-(194)  is 

f(x)   =  6x5-  15t4+  10i3  (195) 

The  expressions  for  payload  reference  trajectory  using  the  fifth  order  polynomial 
become 

ePrrf(t)   =  6p  (tQ)  +  (6x5  -  15i4  +  10t3)  (A9p)  (196) 
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>p     (t)   =   (30t4-60t3  +  30t2)  (A0p) 

ref  * 


0p     (t)   =   (120i3-  180t    +60i)  (A9p) 

ref  ' 


*         1         > 


Vlf      XQJ 


(197) 


(198) 


(tf-t0)" 

The  polynomial  reference  trajectory  is  also  be  applied  to  the  other  generalized 
coordinates  which  form  the  minimum  set  to  describe  the  system  (i.e.  centerbody  attitude 
and  payload  center  of  mass  coordinates).  The  redundant  generalized  coordinates  are  cal- 
culated from  the  reference  coordinates  as  described  earlier. 

Higher  order  polynomials  can  increase  the  complexity  of  the  path  but  offer  the 
advantage  that  an  infinity  of  polynomial  coefficients  satisfy  the  position,  velocity,  and 
acceleration  boundary  conditions.  The  selection  of  the  coefficients  affords  an  opportunity 
to  optimize  the  reaction  wheel  torque.  In  this  system,  manipulator  actuator  torques  are 
internal  while  the  reaction  wheel  torque  is  the  only  external  torque.  Therefore,  the  reac- 
tion wheel  torque  will  be  equal  to  the  rate  of  change  of  angular  momentum  which  can  be 
calculated  directly  from  a  reference  trajectory.    This  technique  is  more  computationally 

efficient  because  it  does  not  require  the  construction  of  the  G  and  b  matrices. 

In  general,  the  angular  momentum  about  the  inertial  origin  for  each  member  of  the 
system  is 

H    =  Leo  +m.(r  xv  )  (199) 

where         I.  is  the  moment  of  inertia  of  the  i    body  about  its  center  of  mass 
m  is  the  angular  rate  of  the  i    body 
mi  is  the  mass  of  the  i    body 

r  is  the  inertial  position  of  the  i    body  center  of  mass 
v  is  the  inertial  velocity  of  the  i    body  center  of  mass 
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The  angular  rate,  position  and  velocity  vectors  were  previously  developed  in  con- 
nection with  determining  kinetic  energy.  Those  expressions  require  some  coordinate 
transformations  to  express  all  the  terms  with  respect  to  the  inertial  coordinate  frame.  The 
change  in  angular  momentum  is  found  by  differentiating  Eq.  (199)  to  produce 

H    =  I  (o   +  m  ( r  x  a  )  (200) 

The  total  system  change  in  angular  momentum  is  the  sum  of  change  in  angular 
momentum  for  each  of  the  members.  After  collecting  terms  with  common  angular  veloc- 
ity or  acceleration  terms,  the  expression  for  the  system  change  in  angular  momentum  is 
given  by 
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H  =  IpBp  +  nip(XpYp-XpYp)  (201) 

•■       r  2  2  2 

+  Hi  i    I.  .  +  I.  t  +  m.  ,1  .  .  +m.  ,1  .  -  +  in,  -I.  .  +  m,  ,  I,  ,1  .  ,cos9,  ,  +  in,  ,1,  J,  .cosO.  , 
Ll  L  LI         L2  LlcLl  L2cL2  L2L!  LILOcL)  LI  L2L0L1  LI 

+  2m,  .I,  .1  ,  ..cosO.  .,  +  m,  ,1.  .1  .  ,cos  (0,     +  0     )  "1 
L2  LI   cL2  L2  L2  LO  cL2  1.1         l2    J 

+  6L2[lL2  +  mL2lc2L2  +  niL2lL1lcL2coseL2  +  mL2lLnlcL2coS(0L1+eL2) 

+  AR.  [lR,  +IR2  +  mRl1cRI  +mR2lcR2+n,R:lk.  +mR.,ROlcRICOS°Rl  +mR2lR0,RlCOs0R. 

+  2mR21RllcR2COs0R2  +  mR2lROlcR2COS(BRl+0R2)] 
+  6R2[lR2  +  niR2lc2R2  +  niR2lR]lcR2cosOR2  +  mR;!R0l  r;coS(0R)+()r:) 

+  eLieL2[-2mL:lL1lcL2si.ieL2-2mL2lLOlcL2sin(0L1+OL2)] 

+  GLI  I"  '"li'lo'cLI  sill°Ll  "  n1L2lLOlLlsin0LI  "  mL2,L0,cL2sin  (°L1  +  °L2>1 

+  0L2[-mL2lL1lcL2si.iGL2-niL2lLOlcI2sin(0L1+eL2)] 

+  GR1GR2  [-  2mR2lR1lcR2sineR2  -  2mR2lR0lcR2sin  (6R|  +0R2)  ] 

+  0R,[-mRIlRolcR1sineR1-niR2lRolR]sin0R1-inR2lROlcR:!sin(OR|+fiR2)] 

+  GR2  [- mR2lR]lcR2sinOR2  -  mR2lR0lcR2sin  (0R1 +HR2)  ] 

Any  polynomial  reference  trajectory  that  satisfies  the  initial  condition  concerning 
displacement  cannot  have  a  constant  term.    Polynomials  which  satisfy  the  velocity  and 

acceleration  initial  conditions  must  not  contain  linear  or  quadratic  terms.  The  general  n 
order  polynomial  reference  trajectory  has  the  form 

f(x)  =  ^xn  +  Vl*""1  +  V2t""2  +  -  +  a5x5  +  a4T4  +  a3i3  (202) 

Derivatives  are 

f  (x)  =  na^""1  +  (n-l)a„.,xn-2  +  (n-2)^.2xn^  +  ...  +  5a5x4  +  4a4x3  +  3a3t2  (203) 
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f'(x)  =  inn-Da,,!""2  +  (n-l)(n-2)an.|iIKl  +  (n-2)(n-3)an.2xn-4  +  ...  +  20a5x5  +  12a4t4  +  6a3x      (204) 
When  t=1  and  the  final  conditions,  f(l)  =  1,  f'(l)  =  0  nad  f"(l)  =  0,  are  substituted 
into  Eqs  (202)-(204),  these  equations  can  be  put  into  matrix  form 


n      I 


1  1  1 

n  (n-1)  (n-2) 

n(n-  1)    (n-  1)  (n-2)    (n-2)  (n-3) 


1  1  1 
5  4  3 
20  12  6 


[W]a 


(205) 


The  column  vector  of  polynomial  coefficients  can  be  partitioned.  One  segment, 
3543,  contains  the  coefficients  for  the  third,  fourth,  and  fifth  order  terms  in  Eq  (202).  The 
other  segment,  a^g^,  contains  all  of  the  coefficients  of  order  six  and  higher. 


high 


543 


(206) 


The  W  matrix  can  be  partitioned  accordingly. 

w  -  [whlgh  w543]  (207) 

One  can  then  solve  for  the  lower  order  polynomial  coefficients  in  terms  of  the 
higher  order  coefficients  by  substituting  Eqs  (206)  and  (207)  into  Eq.  (205).  The  result 
specifies  polynomial  reference  trajectory  coefficients  which  satisfy  the  boundary  condi- 
tions. 


fl543   "   W543 


(r 
1 

0 

lp_ 


W       a 

high    high 


(208) 


An  optimal  solution  for  a  polynomial  reference  trajectory  is  found  by  using  the 
MATLAB  function  fminu.  This  tool  numerically  finds  the  solution  to  an  unconstrained 
function  minimization  problem  using  a  quasi-Newton  method.   The  function  to  be  mini- 
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mized  is  the  rate  of  change  of  angular  momentum,  Eq  (201),  which  can  be  found  once  a 
reference  trajectory  is  specified.  The  user  makes  an  initial  guess  for  the  higher  order  refer- 
ence trajectory  coefficients.  The  lower  order  coefficients  are  calculated  by  Eq.  (208).  The 
MATLAB  function  then  varies  the  higher  order  coefficients  and  recalculates  the  lower 
order  coefficients  as  necessary  to  minimize  change  in  angular  momentum.  One  limitation 
to  this  technique  is  that  the  algorithm  may  converge  to  a  local  rather  than  the  global  mini- 
mum. 
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III.    VALIDATION  AND  SIMULATION  RESULTS 

The  computer  simulations  presented  in  this  chapter  were  obtained  using  the  MATLAB 
subroutines  included  in  Appendix  B.  The  integrator  uses  4  and  5  order  Runge-Kutta 
formulas    See  Appendix  B  for  documented  listings  of  the  computer  code. 

A.  VALIDATION 

To  verify  the  equations  and  find  the  programming  bugs,  test  cases  were  developed. 
The  simulations  are  analyzed  to  ensure  that  universal  principles  such  as  conservation  of 
energy  and  angular  momentum  are  not  violated.  Numeric  values  for  the  generic  dual  two- 
link  manipulator  system  are  contained  in  Table  1 .  The  generic  model  is  the  strawman  con- 
figuration that  all  of  the  test  cases  are  based  on  with  the  exception  of  a  few  minor  varia- 
tions. The  variations  will  be  pointed  out  in  the  appropriate  test  cases.  The  values  for  the 
generic  model's  system  properties  are  picked  for  uniformity  and  simplicity.  The  manipu- 
lator links  and  the  payload  are  modelled  as  slender  rods  of  uniform  density. 

1.    Conservation  of  Kinetic  Energy 

In  the  first  test  case,  no  torques  are  applied  and  the  initial  velocities  are  nonzero. 
Under  these  conditions,  the  system  links  drift  subject  to  the  constraints  of  being  pinned 
together.  Since  potential  energy  is  zero  and  there  are  no  external  energy  sources,  kinetic 
energy  should  remain  constant.  The  system  begins  with  the  payload  parallel  to  a  line 
drawn  between  the  two  shoulders  and  0.75m  away  from  them.  The  initial  angular  rate  for 

the  centerbody  is  chosen  to  be  e0  =  2  deg/sec.  The  initial  angular  rate  for  the  payload  is  eP 
=  -5  deg/sec.  Initial  velocities  for  the  payload  center  of  mass  are  -0.1  m/sec  along  the  x 
axis  and  -0.05  m/sec  along  the  y  axis.  The  remaining  generalized  velocities  are  calculated 
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TABLE  1.    GENERIC  MODEL  SYSTEM  PROPERTIES 


Parameter 

Value 

Length 
(m) 

ho 

0.75 

hi 

0.5 

hi 

0.5 

ho 

0.5 

hi 

0.5 

h2 

0.5 

h 

0.75J2 

Mass 
(kg) 

m0 

5 

™L1 

1 

mL2 

1 

™R1 

1 

mR2 

1 

nip 

1 

Center 

of 

Mass 

(m) 

lc0 

0 

kLI 

0.25 

kL2 

0.25 

km 

0.25 

km 

0.25 

lcP 

0.25 

Moments 

of 

Inertia 

(kg-m2) 

h 

5 

hi 

0.02083 

hi 

0.02083 

hi 

0.02083 

hi 

0.02083 

h 

0.02083 

Shoulder 

Location 

(deg) 

^LO 

90 

0RO 
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based  on  the  values  specified  for  the  centerbody  and  payload.   Initial  angular  rates  for  the 
manipulator  links  are  oLI  =  6.6607  deg/sec,  ()l2  =  -7.0457  deg/sec,  <)R1  =  -2.7553  deg/sec, 

and  oR,  =  14.9127  deg/sec.   The  graphical  results  from  this  test  case  are  included  in  Fig- 
ures 4-8.  As  indicated  in  Figure  7,  kinetic  energy  is  conserved  in  this  case. 
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Figure  4:  Test  Case  1  Angles 
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Figure  5:  Test  Case  1  Angular  Rates 
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Figure  6:  Test  Case  1  Time  Lapse  Stick  Figure 
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Figure  7:  Test  Case  1  Kinetic  Energy 
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Figure  8:  Test  Case  1  Angular  Momentum 

Test  Case  2  is  an  extension  of  Test  Case  1 .  This  is  still  a  case  with  nonzero  initial 
velocities  and  no  external  torques.  However,  the  system  geometry  is  altered  to  be  sym- 
metrical. In  addition  to  conservation  of  kinetic  energy,  this  test  case  will  ensure  that  the 
symmetry  is  preserved.  The  physical  alterations  in  the  system  involve  moving  the  loca- 
tion of  the  left  shoulder  from  90  degrees  to  135  degrees  and  decreasing  the  distance  from 
the  origin  to  the  right  shoulder  to  0.75  meters.  The  payload  still  begins  centered  between 
the  shoulders  and  parallel  to  the  y  axis  but  is  1 .2  m  from  the  origin.  To  maintain  symme- 
try, the  initial  velocities  must  also  be  symmetrical.  The  initial  angular  rate  for  the  center- 
body  is  chosen  to  be  e0  =  0  deg/sec.  The  initial  angular  rate  for  the  payload  is  also  zero. 
Initial  velocities  for  the  payload  center  of  mass  are  zero  along  the  x  axis  and  -0.05  m/sec 
along  the  y  axis.  The  remaining  generalized  velocities  are  again  calculated  based  on  the 
values  specified  for  the  centerbody  and  payload.   Initial  angular  rates  for  the  manipulator 

links  are  6L1  =  2.3188  deg/sec,  eL2  =  -7.6851  deg/sec,  eR,  =  -2.3188  deg/sec,  and  oR2  = 
7.6851  deg/sec.  This  combination  of  system  geometry  and  initial  velocities  is  designed  to 
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cause  the  payload  to  drift  toward  the  origin  without  changing  its  attitude.  Figures  9-13 
show  the  results  from  this  test  case.  Kinetic  energy  is  conserved  and  symmetry  is  pre- 
served. 
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Figure  9:  Test  Case  2  Angles 
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Figure  10:  Test  Case  2  Angular  Rates 
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Figure  11:  Test  Case  2  Time  Lapse  Stick  Figure 
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Figure  12:  Test  Case  2  Kinetic  Energy 
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Figure  13:  Test  Case  2  Angular  Momentum 

2.    Conservation  of  Angular  Momentum 

As  long  as  a  system  does  not  include  external  torques,  one  expects  that  angular 
momentum  should  be  conserved.  The  joint  actuators  provide  internal  torques  while  the 
reaction  wheel  is  the  only  external  source.  Test  Cases  1  and  2  did  not  include  a  reaction 
wheel  and  are  therefore  subject  to  investigation  with  respect  to  conservation  of  angular 
momentum  Both  cases  do  satisfy  the  requirement  as  indicated  by  Figures  8  and  13.  Fur- 
thermore, due  to  the  symmetry  in  the  system  in  Test  Case  2,  the  angular  momentum  of  the 
left  manipulator  links  should  be  cancelled  out  by  the  angular  momentum  of  the  right 
manipulator  links.  The  centerbody  and  payload  should  not  have  any  angular  momentum. 
Consequently,  angular  momentum  for  the  system  should  not  only  be  conserved,  it  should 
be  zero.   Figure  13  show  that  the  angular  momentum  remained  virtually  zero.   The  non- 

1  "7  f\ 

zero  values  of  about  3x10      are  well  within  the  integration  algorithm  tolerance  of  10 

Test  Case  3  returns  to  the  generic  system  from  Table  1 .   Initially,  the  system  is  at 
rest.    Constant  torques  are  applied  at  both  shoulders  and  nowhere  else.   The  torques  are 
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0.01  N-m  applied  in  the  positive  direction  at  the  right  shoulder  and  the  negative  direction 
at  the  left  shoulder.  Because  the  joint  torques  are  internal  to  the  system,  angular  momen- 
tum must  still  be  conserved  even  though  kinetic  energy  won't  be.  Furthermore,  since  the 
system  began  at  rest,  the  angular  momentum  should  remain  at  zero.  The  results  are  shown 
in  Figures  14-18.    Although  the  angular  momentum  did  not  remain  identically  equal  to 

zero,  their  magnitudes  of  less  than  2x10"  are  within  the  10  tolerance  placed  on  the  inte- 
gration algorithm. 
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Figure  15:  Test  Case  3  Angular  Rates 
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Figure  16:  Test  Case  3  Time  Lapse  Stick  Figure 
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Figure  17:  Test  Case  3  Kinetic  Energy 
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Figure  18:  Test  Case  3  Angular  Momentum 

Test  Case  4  is  similar  to  Test  Case  3  but  the  symmetrical  system  geometry  is  used 
instead  of  the  generic  geometry.  This  change  should  produce  symmetric  motion  and  zero 
angular  momentum.  The  reaction  wheel  is  still  disabled.  Figures  19-23  indicate  the  sys- 
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tem  reacted  as  expected.   Changing  the  torques  to  time  varying  profiles  rather  than  con- 
stants led  to  similar  results. 
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Figure  19:  Test  Case  4  Angles 
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Figure  20:  Test  Case  4  Angular  Rates 
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Figure  21:  Test  Case  4  Time  Lapse  Stick  Figure 
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Figure  22:  Test  Case  4  Kinetic  Energy 
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Figure  23:  Test  Case  4  Angular  Momentum 

3.    Wheel  Torque  and  Constraints 

The  remaining  test  cases  involved  using  the  reaction  wheel  on  the  centerbody.  The 
wheel's  function  was  to  maintain  attitude  pointing.  The  system  begins  at  rest.  The  torque 
applied  by  the  wheel  is  an  external  torque  in  this  model.  Therefore,  its  value  must  be  the 
same  as  the  change  in  angular  momentum.  The  wheel  torque  is  found  by  means  of  the 
inverse  kinematics  equations  in  Chapter  II.  These  calculations  are  entirely  independent  of 
finding  the  change  in  angular  momentum.  After  a  simulation  is  finished,  a  separate  sec- 
tion in  the  program  code  calculates  the  change  in  angular  momentum  using  the  general- 
ized coordinates,  velocities  and  accelerations  produced  by  the  integration.  These  values 
are  plotted  along  with  those  of  the  reaction  wheel  torque.  A  sample  plot  is  contained  in 
Figure  24.  This  particular  plot  is  for  the  case  of  a  fifth  order  polynomial  reference  trajec- 
tory. The  rest  of  the  plots  associated  with  this  case  are  presented  later  in  the  Simulations 
section.  The  validation  tests  concerning  conservation  of  kinetic  energy  and  angular 
momentum  required  special  circumstances  to  create  those  conditions.    The  requirement 
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that  the  reaction  wheel  torque  equal  the  change  in  angular  momentum  is  more  universal. 
It  is  a  verification  check  performed  with  every  simulation  involving  a  reaction  wheel. 
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Figure  24:  Sample  of  Wheel  Torque  and  Change  in  Angular  Momentum  vs.  Time 

An  even  more  universal  check  also  performed  with  every  simulation  is  the  require- 
ment that  the  constraint  equations  (Aq  +  A(J  =  o)  are  satisfied.  Figure  25  shows  a  sample 
plot.  This  plot  was  also  taken  from  the  fifth  order  polynomial  reference  trajectory  case. 
The  values  plotted  represent  the  four  constraint  equations  contained  in  Eqn  72.  The  non- 
zero values  are  attributed  to  numerical  errors  created  by  the  integration. 

Finally,  a  common  sense  check  also  performed  with  every  simulation  is  simply  to 
verify  that  the  payload  was  repositioned  to  the  desired  final  location.  This  cannot  happen 
if  the  torques  applied  to  the  system  were  incorrect.  This  test  is  a  necessary  but  not  suffi- 
cient condition  that  the  code  operates  correctly. 
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Figure  25:  Sample  of  Constraints  vs.  Time 

B.  SIMULATIONS 

This  section  presents  results  from  several  simulations  of  an  analytical  model.  The 
desired  payload  repositioning  maneuver  is  illustrated  in  Figure  26.  The  final  position  for 
the  payload  involves  a  90  degree  rotation  and  the  right  endpoint  finishes  where  the  left 
endpoint  started. 

1.    Lyapunov  Point  Controller 

In  the  first  simulation,  the  repositioning  is  done  entirely  by  the  Lyapunov  control- 
ler without  the  benefit  of  a  reference  trajectory.  The  behavior  is  that  of  a  point  controller 
with  an  initial  displacement  rather  than  that  of  a  tracking  controller.  Due  to  the  absence  of 
the  weighted  norm  reference  torques,  this  controller  cannot  be  consider  to  have  coopera- 
tive nature.  Figure  27  presents  the  angular  displacement  history.  The  asterisks  on  the 
right  side  of  the  plot  indicate  the  desired  final  angles  Although  the  system  is  approaching 
the  desired  final  geometry,  it  has  not  completely  settled  down  even  after  40  seconds.  Posi- 
tion errors  (Figure  28)  are  still  present  as  well  as  nonzero  velocities  (Figure  29).  Also  note 
that  the  reaction  wheel  torque  is  quite  high  during  the  maneuver  (Figure  30).   The  joint 
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actuator  torques  are  considerably  less  than  the  reaction  wheel  torque  They  are  not  identi- 
fied individually  because  the  most  important  feature  of  Figure  30  is  the  reaction  wheel 

torque.  As  a  quantitative  measure  of  this  controller's  quality,  j|uwh|dt  produces  a  value  of 

17.3841.  The  oscillatory  nature  of  the  system  is  evident  in  the  angular  position  and  veloc- 
ity plots.  Despite  the  oscillations,  however,  the  stability  of  the  controller  is  also  illus- 
trated. Figure  31  depicts  the  system  geometry  at  several  instances  during  the  maneuver. 
The  left  manipulator  links  actually  cross  over  each  other.  In  experimental  hardware,  the 
links  would  collide  instead.  Figure  32  removes  the  clutter  that  is  present  in  Figure  3 1  and 
displays  only  the  initial  and  final  geometry.  The  Lyapunov  point  controller  also  does  a 
poor  job  of  maintaining  the  centerbody  attitude.  This  is  clearly  evident  in  Figures  27  and 
3 1 .  The  attitude  error  peaks  at  about  1 6  degrees. 
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Figure  26:  Desired  Repositioning  Maneuver 
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Figure  27:  Lyapunov  Point  Controller  Angles 
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Figure  28:  Lyapunov  Point  Controller  Displacement  Errors 
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Figure  29:  Lyapunov  Point  Controller  Angular  Rates 
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Figure  30:  Lyapunov  Point  Controller  Command  Torques 
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Figure  31:  Lyapunov  Point  Controller  Time  Lapse  Stick  Figure 
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Figure  32:  Lyapunov  Point  Controller  Initial  and  Final  Stick  Figures 
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2.    Lyapunov  Tracking  Controller 

This  controller  uses  the  following  equation  to  calculate  control  torques 

u  =  C1t{-Kv5q  +  C,     u      -(Cq-C      q     )-(C    -C3    )-K5q}        (209) 

i  v  irerrel  <*•-  -rcf  rel  Jref  i 

This  equation  was  developed  in  the  analytical  chapter  and  repeated  here  for  conve- 
nience. The  command  torques  are  based  on  errors  with  a  reference  trajectory.  Reference 
torques  which  resulted  from  minimizing  a  weighted  norm  of  the  actuator  torques  associ- 
ated with  the  reference  trajectory  are  also  included. 

a.    5    Order  Reference  Trajectory 

In  this  simulation,  a  fifth  order  polynomial  reference  trajectory  is  applied  to  the 
payload  generalized  coordinates.  The  payload  coordinates  displacements,  velocities,  and 
accelerations  resulting  from  this  polynomial  are  depicted  in  Figure  33.  When  calculating 
the  reference  torques  from  the  inverse  kinematics,  the  six  joint  actuators  are  all  weighted 
equally  The  maneuver  time  is  selected  to  last  10  seconds.  As  is  demonstrated  in  Figures 
34-36,  the  system  successfully  moves  from  initial  conditions  to  desired  final  conditions. 
The  displacement  errors  are  less  than  10  deg  (Figure  34).  The  command  torques  (Figure 
37)  are  an  order  of  magnitude  smaller  than  for  the  previous  simulation  which  lacked  a 
reference  trajectory.  Evaluating  J|uwh|di  leads  to  the  dramatically  improved  value  of 
0.5746.  More  importantly,  the  centerbody  attitude  is  maintained  throughout  the  maneuver. 
Figure  38  shows  the  time  lapse  depiction  of  the  maneuver. 
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Displacement  vs  Normalized  Time 
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Figure  33:  5     Order  Reference  Trajectories 
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Figure  34:  5th  Order  Angles 


66 


Displacement  Errors  vs  Time 
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Figure  35:  5     Order  Displacement  Errors 
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Figure  36:  5     Order  Angular  Rates 
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Figure  37:  5     Order  Command  Torques 
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Figure  38:  5     Order  Time  Lapse  Stick  Figure 
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h.    8    Order  Reference  Trajectory 

By  increasing  the  order  of  the  reference  trajectory  polynomial  while 
maintaining  the  same  boundary  conditions  concerning  velocity  and  acceleration,  one 
hopes  to  achieve  improved  performance.  For  example,  the  domain  of  all  sixth  order 
polynomial  functions  includes  all  fifth  order  polynomial  functions  as  a  subset.  Therefore, 
when  searching  all  sixth  order  polynomials  for  coefficients  which  will  minimize  the  cost 
function,  one  possible  solution  is  the  fifth  order  polynomial  already  used.  Using  the 
function  minimization  routine  discussed  in  the  previous  chapter,  a  sixth  order  polynomial 
function  was  found.  Although  there  was  some  improvement,  the  change  in  performance 
was  not  significant.  The  same  was  true  for  a  seventh  order  function.  An  eighth  order 
function  is  presented  here.  It  was  hoped  that  the  increased  order  would  be  enough  of  a 
departure  from  the  fifth  order  cause  to  produce  significant  improvement  in  reducing  the 
centerbody  disturbance  torque.  The  algorithm  converged  to  a  solution  for  the  eighth  order 
polynomial  after  running  approximately  two  hours  on  a  personal  computer  with  an  Intel 
486-DX50  cpu.  The  resulting  trajectories  are  very  similar  to  those  for  the  fifth  order  case 
and  are  displayed  in  Figure  39.  The  most  obvious  difference  is  a  lack  of  symmetry.  Plots 
for  this  case  are  contained  in  Figures  40-43.  The  value  of  J|uwh|dt  for  this  case  was  0.5705. 
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Figure  39:  8     Order  Reference  Trajectories 


Angles 
(deg) 


100 

50 

0 

-50 


■100    - 


-150 


Thetas  vs 

Time 

i 

V-- 

-  5 : 

- 

°LJ_ 

-r^^^Ri 

— , 

"<^ 

% 

- 

i 

— r-*: 

10 


Time  (sec) 
Figure  40:  8th  Order  Angles 
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Figure  41:  8     Order  Angular  Rates 
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Figure  43:  8     Order  Time  Lapse  Stick  Figure 


3.    Modified  Lyapunov  Tracking  Controller 

This  simulation  represents  a  compromise  between  the  Lyapunov  point  controller 
and  the  Lyapunov  tracking  controller.  Because  the  Lyapunov  point  controller  does  not  use 
a  reference  trajectory,  the  cost  function  which  minimizes  the  weighted  norm  of  the  actua- 
tor torques  is  completely  bypassed.  The  modified  Lyapunov  controller  removes  the  refer- 
ence torque  term  from  the  command  torque  calculation  (Eqn  209)  but  calculates  command 
torques  based  on  errors  with  a  reference  trajectory.  Like  the  Lyapunov  point  controller, 
the  modified  Lyapunov  tracking  controller  does  not  minimize  a  weighted  norm  of  the 
actuator  torques  and  is  therefore  not  a  cooperative  controller.  The  angle  histories  in  Figure 
44  exhibit  less  of  the  oscillatory  nature  than  the  point  controller  simulation,  but  the  accu- 
racy shown  in  Figure  45  is  considerably  worse  than  the  reference  trajectory  simulations. 
Figures  46-48  also  illustrate  behavior  better  than  the  point  controller  but  not  as  good  as 
when  command  torques  are  found  using  Eqn  209.  The  magnitude  of  the  command  torques 
show  an  order  of  magnitude  improvement  over  the  point  controller  This  is  directly  attrib- 
utable to  using  intermediate  reference  points  on  the  way  to  a  desired  final  state  rather  than 
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attempting  to  achieve  the  desired  final  state  all  at  once.    Calculating  J|uwh|di  produced  a 

value  of  2.4523.   The  time  lapse  figure  shows  that  the  motion  is  much  less  wild  but  the 
centerbody  attitude  error  is  still  noticeable. 
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Figure  44:  Modified  Lyapunov  Tracking  Controller  Angles 
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Figure  45:  Modified  Lyapunov  Tracking  Controller  Displacement  Errors 
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Figure  46:  Modified  Lyapunov  Tracking  Controller  Angular  Rates 
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Figure  47:  Modified  Lyapunov  Tracking  Controller  Command  Torques 
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Figure  48:  Modified  Lyapunov  Tracking  Controller  Time  Lapse  Stick  Figure 

4.    Comparison  of  Controllers 

Table  2  summarizes  the  results  of  the  Lyapunov  point  controller,  the  two  Lyapunov 
tracking  controller  cases,  and  the  modified  Lyapunov  tracking  controller.  The  point  con- 
troller clearly  has  the  worst  performance  with  high  reaction  wheel  torque  and  large  center- 
body  attitude  error.  The  tracking  controller  performs  much  better.  Reaction  wheel  torque 
is  greatly  reduced  and  centerbody  attitude  error  is  eliminated.  As  expected,  increasing  the 
order  of  the  polynomial  reduces  the  reaction  wheel  torque  further,  but  the  improvement  is 
relatively  small.  The  modified  tracking  controller  strikes  a  compromise  between  the  point 
controller  and  the  tracking  controller. 
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TABLE  2.      COMPARISON  OF  HYPOTHETICAL  MODEL 
SIMULATIONS 


Khldt 

I     mail 

Centerbody 

Attitude 
Error  (deg) 

Cooperative 

Point  Controller 

17.3841 

2.9365 

16.2261 

No 

Tracking 
Controller 

5th  Order 

0.5746 

0.0961 

0.0000 

Yes 

8lh  Order 

0.5705 

0.0885 

0  0000 

Yes 

Modified  Tracking 
Controller 

2.4523 

0.3950 

1.1910 

No 
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IV.    EXPERIMENTAL  WORK 

The  experimental  phase  of  this  research  was  conducted  on  the  Spacecraft  Robotics 
Simulator  (SRS).  The  SRS  is  a  derivative  of  the  Flexible  Spacecraft  Simulator  (FSS)  ini- 
tially developed  by  Watkins  [Ref  17]  and  later  modified  by  Hailey  [Ref  18].  Sorensen 
[Ref  1 8]  began  the  work  to  convert  the  FSS  into  the  SRS. 

A.  SETUP 

The  SRS  permits  experimental  investigation  of  two  dimensional  robotics  motion  and 
rotational  spacecraft  dynamics.  The  SRS  is  illustrated  in  Figures  49  and  50.  The  simula- 
tor hardware  is  floated  on  an  eight  foot  by  six  foot  granite  table  by  means  of  a  thin  layer  of 
air  supplied  by  an  external  source.  The  table  is  polished  to  within  0.001  inch  peak  to  val- 
ley and  leveled  to  prevent  gravitational  accelerations  from  impacting  the  motion  across  its 
surface.  The  following  sections  describe  the  simulated  spacecraft  with  its  associated  sen- 
sors and  actuators  and  the  controller  which  together  form  the  SRS.  The  spacecraft  compo- 
nents are  the  centerbody,  two  manipulators,  and  a  payload. 
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Figure  49:  Spacecraft  Robotics  Simulator 
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Figure  50:  System  Top  View 

1.    Centerbody 

The  centerbody  is  a  30  inch  diameter,  7/8  inch  thick  aluminum  disk.  The  center- 
body  carries  a  position  sensor,  rate  sensor,  momentum  wheel,  thrusters,  and  an  air  tank  to 
power  the  thrusters.  The  centerbody  also  serves  as  the  mounting  platform  for  the  manipu- 
lators. The  centerbody  is  floated  by  a  central  air  bearing  and  three  air  pads  located  at  120 
degree  intervals  near  the  outer  edge.  The  air  pads  are  each  capable  of  floating  60  pounds 
when  the  air  pressure  supplied  to  the  pads  is  80  psi  The  air  bearing  is  attached  to  an  over- 
head I-beam  which  restricts  to  motion  of  the  centerbody  to  rotation  only. 
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Centerbody  angular  position  is  sensed  by  a  Rotary  Variable  Displacement  Trans- 
ducer (RVDT)  mounted  directly  above  the  air  bearing.  The  RVDT  is  a  model  R30D  man- 
ufactured by  Schaevitz  Sensing  Systems.  Its  linear  range  is  restricted  to  ±  40  degrees. 
Centerbody  angular  rate  is  measured  by  a  rate  transducer  manufactured  by  Humphrey,  Inc. 
The  instrument  has  a  range  of  ±100  deg/sec  and  a  minimum  threshold  of  0.01  deg/sec. 

Centerbody  angular  position  is  controlled  by  a  momentum  wheel.  The  momentum 
wheel  speed  is  measured  by  a  tachometer  contained  in  the  servo  motor  which  drives  the 
momentum  wheel.  The  centerbody  momentum  wheel  is  powered  by  a  model  JR16M4CH/ 
F9T  servo  motor  manufactured  by  PMI  Industries.  Characteristics  of  this  motor  are  sum- 
marized in  Table  3.  Although  the  centerbody  also  carries  two  thrusters,  they  are  not  used 
in  this  research. 

TABLE  3.  MOMENTUM  WHEEL  MOTOR 
CHARACTERISTICS 


Manufacturer 

PMI  Industries 

Model 

JR16M4CH/F9T 

Rated  Output  Speed  (rpm) 

3000 

Rated  Current  (amps) 

7.79 

Rated  Voltage  (volts) 

168 

Rated  Torque  (in-lb) 

31.85 

Height  (in) 

4.5 

Weight  (lb) 

17.5 

Outside  Diameter  (in) 

7.4 

2.    Manipulators 

Two  two-link  manipulators  are  mounted  60  degrees  apart  on  the  centerbody.  Each 
manipulator  has  three  joints.  The  shoulder  joints  are  supported  by  the  centerbody  while 
the  elbow  and  wrist  joints  are  supported  by  two  air  pads  apiece.   The  links  between  the 
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joints  are  stiff  laterally  but  permit  some  flexibility  vertically    This  feature  increases  the 
tolerances  on  the  air  pad  height  adjustment 

Left  arm  joint  angles  are  measured  by  the  same  model  RVDT  as  is  used  on  the  cen- 
terbody  All  three  of  the  left  arm  actuators  are  series  9FGHD  servo  disk  motors  manufac- 
tured by  PMI  Industries.  Joint  angles  on  the  right  arm  are  sensed  by  encoders  purchased 
with  the  joint  actuators.  The  encoder  resolution  is  0.005  degrees.  The  right  arm  joint 
actuators  arm  are  harmonic  drive  dc  servo  actuators  manufactured  by  HD  Systems,  Inc. 
The  shoulder  actuator  is  model  RFS-25-6018-E036AL  while  the  elbow  and  wrist  actua- 
tors are  model  RFS-20-6012-E036AL.  Specifications  for  the  three  types  of  joint  actuators 
are  contained  in  Table  4. 


Air  Pad 


Figure  51:  Left  Manipulator  Top  and  Side  Views 
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Actuator/Encoder 


Air  Pad 


Figure  52:  Right  Manipulator  Top  and  Side  Views 


TABLE  4.  MANIPULATOR  ACTUATOR  CHARACTERISTICS 


Manufacturer 

HD  Systems 

HD  Systems 

PMI  Industries 

Model 

RFS-25-6012 

RFS-25-6018 

9FGHD 

Reduction  Ratio 

1:50 

1:50 

1:148.5 

Rated  Output  Speed  (rpm) 

60 

60 

17 

Rated  Current  (amps) 

2.9 

3.9 

5.6 

Rated  Voltage  (volts) 

75 

75 

12 

Rated  Torque  (in-lb) 

174 

260 

80 

Height  (in) 

8.8 

9.6 

3 

Weight  (lb) 

9.3 

14.1 

3.2 

Footprint  (in) 

4.3<!> 

5.1") 

48(2) 

1  Side  of  square 

2  Diameter  of  circle 


82 


The  joint  actuators  are  all  driven  by  Kepco  power  supplies.  These  bipolar,  pro- 
grammable, linear  amplifiers  can  be  controlled  manually  from  the  front  panel  or  con- 
trolled remotely  with  a  ±10  volt  signal.  In  this  application,  the  power  supplies  are 
operated  in  the  current  control  mode  with  the  voltage  and  current  limits  manually  set  con- 
sistent with  the  values  in  Table  4.  The  specific  power  supply  models  and  their  characteris- 
tics are  summarized  in  Table  5. 

TABLE  5.  POWER  SUPPLIES  CHARACTERISTICS 


Model 

BOP  72-6M 

BOP  72-3  M 

BOP20-10M 

Actuators  Controlled 

Right  Shoulder 

Right  Elbow, 
Right  Wrist 

All  Left  Arm 
Joints 

DC  Output  Range 

±72  volts 
±6  amps 

±72  volts 
±3  amps 

±20  volts 
±10  amps 

Closed  Loop  Gain 

0.6  (amp/volt) 

0.3  (amp/volt) 

1.0  (amp/volt) 

3.  Payload 

The  payload  is  a  rigid  bar  mechanically  fastened  to  the  ends  of  both  manipulators. 
The  payload  is  supported  entirely  by  the  air  pads  on  the  manipulator  wrist  joints.  Ballast 
can  be  added  to  the  payload  to  change  the  mass  and  inertia  characteristics  of  the  system. 
This  allows  for  the  construction  of  cases  in  which  the  mass  of  the  payload  is  nontrivial 
compared  to  the  spacecraft  centerbody.  The  payload  contains  no  sensors  or  actuators. 
Payload  position  is  derived  from  the  manipulator  joint  angles. 

4.  Controller 

The  AC-100  programmable  controller  manufactured  by  Integrated  Systems,  Inc. 
controls  the  SRS.  The  AC-100  includes  an  Intel  80386  Application  Processor,  an  Intel 
80386  Multibus  II  Input/Output  Processor,  an  Intel  80386  Communication  Processor,  and 
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Intel  80387  Coprocessor,  a  Weitek  3167  Coprocessor,  and  Analog-To-Digital  and  Digital- 
To- Analog  Data  Translation  DT2402  Input/Output  Board,  two  INX-04  Encoder  and  Digi- 
tal-To-Analog  Servo  Boards,  and  an  Ethernet  Interface  Module  The  AC-100  also 
includes  software  installed  on  a  VAX  3100  Series  Model  30  workstation.  The  software 
permits  design  of  a  controller  in  block  diagram  graphical  form  and  conversion  of  the  dia- 
gram to  C  language  programming  code.  The  user  is  also  able  to  design  an  interactive  ani- 
mation window  to  operate  the  controller.  The  AC-100  receives  input  signals  from  the 
sensors  and  the  graphical  user  interface.  AC-100  output  signals  go  to  the  power  supplies 
driving  the  actuators  or  to  the  graphical  user  interface  for  display. 
5.    System  Integration 

The  differences  between  the  ideal  world  of  an  analytical  simulation  and  the  real 
world  of  actual  hardware  became  apparent  during  system  integration.  A  few  problems 
arose  then  requiring  some  modification  of  the  experiment.  The  first  problem  concerned 
floating  the  centerbody.  It  exhibited  a  noticeable  resistance  to  rotation.  This  is  due  in  part 
to  the  air  pressure  of  the  available  air  supply.  Because  it  was  only  40  psi,  the  air  pads  per- 
formance was  degraded  by  a  factor  of  two.  Prior  to  mounting  the  manipulators,  the  cen- 
terbody weighed  approximately  125  lbs.  Adding  the  shoulder  motors  increased  the 
centerbody  weight  to  145  lbs.  The  extra  weight  may  have  been  enough  to  overwhelmed 
the  centerbody  air  pads.  A  second  contributing  factor  to  the  centerbody  drag  is  the  inabil- 
ity of  the  central  air  bearing  to  function  except  under  very  low  lateral  loading.  The  modi- 
fication to  the  experiment  created  by  the  centerbody  problem  is  to  not  float  the  centerbody. 

A  second  problem  involved  using  the  RVDTs.  As  envisioned,  the  experiment 
requires  one  RVDT  for  the  centerbody  and  three  for  the  left  manipulator  joints.  The  Space 
Dynamics  laboratory  has  a  total  of  three  in  stock.  Although  a  fourth  has  been  ordered,  it 
did  not  arrive  in  time  to  be  used.  Using  the  existing  RVDTs  revealed  another  problem. 
Data  acquisition  of  the  RVDT  signal  by  the  AC-100  exhibited  a  random  toggling  of  the 
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sensed  value  between  a  good  reading  and  a  value  of  zero.  Because  the  angle  information 
is  critical  to  calculating  actuator  commands,  this  behavior  is  unacceptable.  Consultation 
with  the  Integrated  Systems  technical  support  group  revealed  that  this  type  of  behavior  is  a 
bug  within  the  AC-100  software  which  has  been  corrected  in  more  recent  versions.  Use  of 
the  newer  version  was  not  possible  because  it  requires  upgrading  the  VAX  workstation 
hardware  and  an  updated  version  of  the  VMS  operating  system.  The  experimental  modifi- 
cation used  to  overcome  these  difficulties  is  to  derive  the  joint  angles  and  velocities  of  the 
left  manipulator  by  using  the  sensed  information  from  the  right  manipulator  encoders. 
Velocities  were  not  sensed  directly  but  approximated  by  the  change  in  displacement  which 
occurred  since  the  last  sample  divided  by  the  sample  rate 

A  third  obstacle  involved  the  limitations  of  software  to  design  the  control  algo- 
rithm. The  block  diagram  construction  method  did  not  permit  convenient  matrix  opera- 
tions. Matrix  multiplication  must  be  programmed  in  an  element  by  element  basis.  Matrix 
inversion  must  also  be  calculated  by  constructing  a  series  of  blocks  to  find  each  element. 
This  handicap  is  not  serious  when  the  system  equations  of  motion  are  of  low  order.  How- 
ever, the  dual  two  link  manipulator  configuration  is  eighth  order  and  beyond  the  practical 
means  of  programming  complex  matrix  operations,  especially  matrix  inverses.  Recall  that 
the  command  torques  are  calculated  by  the  following  relationship 
u  =    (C[C  )_1c]VKv5q  +  C      u      "(Cq-C      q     )  -  (C    -  C,    )-K5q} 

(210) 
When  the  differences  between  the  actual  path  and  the  reference  path  are  small,  this  control 
law  simplifies  to  something  very  similar  to  a  PD  controller.  Therefore,  the  control  law 
used  by  the  experiment  is  a  PD  controller  rather  than  the  complete  Lyapunov  controller. 

Performance  differences  between  the  left  and  right  manipulator  actuators  also  pre- 
sented some  problems.  Because  of  the  actuator  redundancy,  any  three  joint  actuators 
should  be  enough  to  follow  a  reference  trajectory.  This  fact  can  be  demonstrated  by  using 
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only  the  three  right  joint  actuators.  However,  the  same  trajectory  is  not  possible  with  only 
the  left  actuators.  The  torque  provided  by  the  left  joint  actuators  is  insufficient  to  com- 
pletely overcome  the  internal  friction  of  the  right  joint  actuators.  Even  when  the  left  joint 
actuators  are  commanded  manually  from  the  front  panel,  there  is  no  correction  to  reduce 
the  position  error.  When  steadily  increasing  the  commanded  current  to  the  motor,  the  cur- 
rent limit  is  reached  before  the  motor  responds. 

B.   RESULTS 

The  reference  trajectory  for  the  experimental  phase  is  slightly  different  from  that  used 
in  the  analytical  section.  The  reference  maneuver  still  involves  a  90  degree  rotation  of  the 
payload  with  the  right  endpoint  ending  where  the  left  endpoint  began.  The  differences 
arise  from  the  system  parameter  such  as  lengths  and  masses  not  being  the  same  as  in  the 
generic  hypothetical  model.  The  desired  reference  maneuver  is  depicted  in  Figure  53. 
Results  are  shown  in  Figures  54-58  and  summarized  in  Table  6.  The  sudden  changes  from 
believable  values  to  zero  in  the  figures  are  problems  with  the  data  acquisition  software  and 
do  not  indicate  actual  changes  in  the  experimental  hardware  geometry. 
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Figure  53:  Desired  Experimental  Repositioning  Maneuver 
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TABLE  6.  EXPERIMENTAL  ERROR  ANGLES 


Errors  (deg) 

Initial 

Final 

Maximum 
Magnitude 

0p 

©Rl 
6R2 

0.2550 
-0.4574 
0.0225 
0.1037 
0.3350 

-0.3383 
0.0366 
0.1873 
-0.0808 
0.3950 

0.5527 
0.7797 
0.3035 
0.1628 
0.7742 
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V.    SUMMARY  AND  CONCLUSIONS 

A.  SUMMARY 

The  dynamics  of  a  dual  two-link  manipulator  system  which  is  repositioning  an  already 
grasped  payload  have  been  analyzed.  The  equations  of  motion  for  the  system  were  devel- 
oped using  Lagrange's  method.  The  resulting  equations  were  highly  nonlinear,  coupled, 
second  order  differential  equations.  Given  any  reference  trajectory,  the  actuator  torques 
that  will  produce  that  trajectory  were  calculated  to  minimize  a  weighted  norm  of  the 
torques.  Stability  of  the  system  during  the  repositioning  maneuver  was  ensured  by  a  con- 
troller derived  from  Lyapunov  stability  theory.  Equations  for  deriving  joint  angles  from 
centerbody  and  payload  reference  values  was  also  developed.  Polynomial  reference  tra- 
jectories were  presented  as  an  attractive  means  to  specify  a  reference  trajectory. 

The  analytical  model  was  validated  using  test  cases  in  which  some  results  could  be 
predicted  in  advance.  The  model  demonstrated  conservation  of  energy  when  no  torques 
were  applied.  It  also  exhibited  conservation  of  angular  momentum  whenever  the  reaction 
wheel  was  disabled.  The  model  also  maintained  symmetric  geometry  in  the  appropriate 
test  cases.  In  cases  which  used  the  reaction  wheel,  conservation  of  energy  and  angular 
momentum  did  not  apply,  However,  comparison  of  the  change  in  angular  momentum  with 
the  reaction  wheel  torque  provided  validation.  Finally,  in  all  test  cases  as  well  as  simula- 
tions, the  constraints  were  satisfied  as  measured  by  Aq+A0  =  o. 

Results  from  simulations  indicated  that  the  Lyapunov  point  controller,  although  stable, 
behaved  poorly.  Large  centerbody  attitude  errors,  high  command  torques,  and  wild  oscil- 
lations make  this  controller  undesirable  for  large  repositioning  maneuvers.  The  Lyapunov 
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tracking  controller  exhibited  dramatic  a  improvement  in  performance.  Centerbody  atti- 
tude errors  were  removed  and  reaction  wheel  torque  decreased  significantly. 

The  experimental  phase  revealed  that  the  controller  required  further  simplification  for 
compatibility  with  the  laboratory  resources.  Acceptable  results  were  obtained  using  a  PD 
control  law  with  a  reference  trajectory. 

The  objectives  of  this  research  were  to  1)  develop  a  stable  control  law  that  facilitates 
cooperation  among  the  manipulators  as  they  reposition  the  payload,  2)  minimize  the  joint 
actuator  effort,  3)  reduce  the  disturbance  torque  transmitted  to  the  spacecraft  main  body 
by  the  manipulator  motion,  and  4)  validate  the  analytical  development  with  experimental 
results.  The  Lyapunov  controller  satisfies  the  first  objective.  The  second  objective  is 
achieved  by  the  weighted  norm  calculation  of  the  actuator  torques.  Reduction  of  the  cen- 
terbody disturbance  torque  is  accomplished  through  reference  trajectory  selection. 
Although  a  rigorous  application  of  classical  optimal  control  techniques  proved  impracti- 
cal, a  polynomial  reference  trajectory  in  which  the  coefficients  were  selected  to  reduce  the 
disturbance  torque  was  easily  applied.  Difficulties  were  encountered  with  regards  to  the 
fourth  objective,  experimental  work.  The  controller  developed  analytically  could  not  be 
directly  transferred  to  the  laboratory.  This  was  due  to  a  combination  of  hardware  limita- 
tions and  real  world  conditions  instead  of  the  ideal  environment  of  the  analytical  model. 
The  controller  was  adapted  to  the  realities  of  the  laboratory  and  resulted  in  successful 
accomplishment  of  a  payload  repositioning  maneuver. 

B.  ORIGINAL  CONTRIBUTIONS 

A  simulation  tool  has  been  developed  to  analyze  the  dynamics  of  a  space  based  robot- 
ics system.  Some  of  the  features  of  this  tool  include: 
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(i)  rotational  motion  of  the  spacecraft  centerbody  and  planar  motion  of  the  manip- 
ulators and  payload; 

(ii)  minimization  of  a  weighted  norm  of  the  actuator  torques  based  on  a  user  sup- 
plied weighting  matrix; 

(iii)  calculation  of  polynomial  reference  trajectory  coefficients  to  produce  a  local 
minimum  for  the  integral  of  the  absolute  value  of  the  disturbance  torque  based 
on  a  user  supplied  order  for  the  reference  polynomial  and  an  initial  guess  for 
the  coefficients; 

(iv)  a  reference  trajectory  with  zero  velocity  and  acceleration  at  the  beginning  and 
end  of  the  maneuver; 

(v)        a  Lyapunov  controller  which  guarantees  stability  in  the  face  of  perturbations 

between  the  reference  trajectory  and  the  actual  dynamics  caused  by  errors  in 

the  initial  conditions. 

An  experimental  test  bed  was  also  developed.   This  effort  involved  the  design  of  the 

manipulator  components  and  the  development  of  a  real  time  controller.    This  test  bed 

remains  in  the  Spacecraft  Dynamics  and  Control  Laboratory  and  is  available  for  follow-on 

work. 

C.  RECOMMENDATIONS  FOR  FURTHER  STUDY 

As  with  any  research,  this  work  answers  some  questions  but  raises  others.  One  of  the 
areas  that  could  receive  further  attention  is  the  selection  of  the  Lyapunov  controller  gains. 
The  theory  requires  positive  definite  matrices  to  ensure  stability  but  offers  no  insights  con- 
cerning selection  of  the  matrices  to  improve  performance.  For  any  given  set  of  controller 
matrices,  one  expects  the  relative  merits  of  the  point  controller,  tracking  controller,  and 
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modified  tracking  controller  to  remain  the  same  However,  still  better  performance  might 
be  achieved  across  the  board  if  the  gains  were  optimized. 

Rather  than  merely  changing  Lyapunov  controller  gains,  one  might  investigate  another 
Lyapunov  controller  by  beginning  with  a  different  candidate  Lyapunov  function  than  the 
one  presented  here  The  choices  are  infinite  and  the  results  and  performance  difficult  to 
predict. 

Trajectory  optimization  is  another  area  that  would  benefit  from  further  work.  The 
function  minimization  algorithm  used  to  select  polynomial  coefficients  converged  to  local 
minima  solutions  depending  on  the  initial  guess  for  the  coefficients.  The  search  for  a  glo- 
bal minimum  for  a  particular  order  polynomial  requires  further  investigation.  An  alternate 
approach  with  respect  to  trajectory  optimization  is  to  use  some  function  other  than  a  sim- 
ple polynomial  to  describe  the  trajectory.  Possible  trajectories  might  be  Tchebycheff  poly- 
nomials, Legendre  polynomials,  or  Fourier  series. 

To  help  bridge  the  gap  between  the  analytical  model  and  the  real  world  hardware,  one 
could  consider  modifying  the  controller  to  include  joint  friction,  actuator  backlash,  sensor 
noise,  and  flexibility.  One  could  also  consider  using  a  minimum  generalized  coordinate 
formulation.  One  might  also  attack  the  differences  from  the  hardware  perspective  by 
seeking  components  that  more  closely  resemble  those  in  the  analytical  model.  Another 
improvement  in  the  experiment  would  be  to  replace  the  existing  joint  velocity  approxima- 
tions with  either  an  observer  or  an  actually  velocity  measurement. 

Finally,  it's  a  three  dimensional  world.  Extending  the  analytical  mode!  and,  if  possi- 
ble, the  laboratory  experiment  to  include  out  of  plane  motion  should  be  considered. 
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APPENDIX  A:    EXPERIMENTAL  CONTROL  BLOCK 

DIAGRAMS 

This  appendix  includes  the  block  diagrams  of  the  System  Build  super  blocks  made  to 
control  the  SRS.  The  heirarchy  among  the  super  blocks  is  illustrated  in  Figure  59.  Both  is 
the  parent  superblock.  The  ohters  are  lower  level  super  blocks. 


Both 


Parameters 

Trajectories 
Encoders 

LeftAngles  - 
Controller 


Parti 

Part2 
Part3 


Figure  59:  Super  Blocks  Hierarchy 

The  block  diagram  for  super  block  Both  is  shown  in  Figure  60.  Inputs  into  the  dia- 
gram include  the  sensor  signals  from  the  hardware  and  user  operated  dials  to  select  the 
controller  gains  and  enable  switches  which  select  the  combination  of  joint  actuators  to 
enable.  The  outputs  include  commanded,  reference,  and  error  signals  for  each  of  the  cen- 
terbody  angle,  joint  angles,  and  payload  angle    Motor  current  commands  to  the  Kepco 


97 


power  supplies  are  also  outputs.  Block  56  contains  the  system  parameter  values  for  the 
experimental  hardware.  This  block  is  expanded  and  displayed  in  Figure  61 .  Block  8  con- 
tains the  position  and  velocity  values  for  a  reference  trajectory  in  a  look-up  table.  It  also 
contains  a  table  to  reset  the  system  back  to  its  original  geometry  to  permit  rerunning  the 
reference  trajectory.  This  block  is  expanded  in  Figure  62.  Conversion  of  the  encoder 
pulses  from  the  right  manipulator  into  angle  and  angle  rate  information  is  done  in  Block  7 
which  is  expanded  in  Figure  64.  Conversion  of  the  encoder  pulses  from  the  right  manipu- 
lator into  angle  information  for  the  left  manipulator  is  done  in  Block  49.  Details  of  this 
block  are  shown  in  Figures  64-64.  The  PD  controllers  which  convert  the  error  signals  into 
actuator  commands  are  in  Block  40.  This  block  is  expanded  in  two  parts.  The  actuator 
commands  for  the  right  and  left  manipulator  are  shown  in  Figures  68  and  69  respectively. 
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Figure  60:  Overall  Control  Block  Diagram 
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Figure  61:  Parameters  Block  Diagram 
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Figure  62:  Reference  Trajectory  Block  Diagram 
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Figure  63:  Encoders  Block  Diagram 
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Figure  64:  Left  Angles  Block  Diagram 
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Figure  65:  Part  1  Block  Diagram 
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Figure  66:  Part  2  Block  Diagram 
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Figure  67:  Part  3  Block  Diagram 
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Figure  68:  Right  Manipulator  Controller  Block  Diagram 
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Figure  69:  Left  Manipulator  Controller  Block  Diagram 
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APPENDIX  B:     MATLAB  CODE 

The  following  listings  are  the  MATLAB  code  used  for  the  analytical  simulations.  The 
modules  are  included  in  alphabetical  order.  The  hierarchical  relationship  between  the 
modules  is  illustrated  in  Figure  70.  The  integration  modules  ode2  and  odemin  are  minor 
variants  of  the  MATLAB  supplied  module  ode45  The  modifications  permit  more  param- 
eters to  be  passed  to  and  from  these  modules  without  having  to  include  the  extra  variables 
in  the  state  vector.  Similarly,  fminu2  modified  the  MATLAB  unconstrained  function  min- 
imization module,  fminu,  to  include  some  diagnostic  statements  while  running. 


MainOpt 


fminu2 


MainMin 


odemin 


Main2 


RefMin2 


ode2 


AngMo2 


Draw3  Eqn2 

AngMo  Ref2 


Matx 


MatxFix 


Matx 


MatxFix 


Matx 


MatxFix 


AngMo2  AngMo2 


Figure  70:  MATLAB  Modules  Hierarchy 
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A.  AngMo 

%  Filename  is  "AngMo. m" 

%  This  file  calculates  the  angular  momentum  of  the  system 

function  [Hs]  =  AngMo(Ls,Ms,CMs,Is,Q,Qdot) 

%  OUTPUT: 

%  Hs  =  1x7  row  vector  of  angular  velocities.  The  first  element  is  for 

%  the  centerbody.  The  next  four  elements  are  for  the  left  upper 

%  and  lower  arm  followed  by  the  right  upper  and  lower  arm    The 

%  last  two  elements  are  for  the  pavload  and  a  total  of  all  the 

%  previous  elements.  [HO  HL 1  HL2  HR1  HR2  1  IP  I  ITotal] 

% 

%  INPUT: 

%  Ls  =  7x1  vector  of  lengths  (m) 

%  1st  element  =  distance  from  origin  to  left  arm  mount 

%  2nd  &  3rd  elements  wrt  left  arm  (from  shoulder  toward  wrist) 

%  4th  element  =  pay  load  length 

%  5th  &  6th  elements  wrt  right  arm  (from  wrist  toward  shoulder) 

%  7th  element  =  distance  from  right  arm  mount  to  origin 

%  [L0;L1;L2;LP;R2;R1;R0] 

%  Ms  =  6x  1  column  vector  containing  the  masses  (kg) 

%  1  st  element  =  mass  of  spacecraft  centerbody 

%  2nd  &  3rd  elements  =  mass  of  left  arm  (upper  aim  then  lower  aim) 

%  4th  &  5th  elements  =  mass  of  right  arm  (upper  arm  then  lower  arm) 

%  6th  element  =  pay  load  mass 

%  [MO;  ML  1 ;  ML2;  MR  1 ;  MR2;  MP] 

%  CMs  =  6x1  column  vector  containing  center  of  mass  locations 

%  [LcO;  LcL  1 ;  LcL2;  LcR  1 ;  LcR2;  LcP] 

%  Is  =  6x1  column  vector  containing  the  moments  of  inertias  about  the 

%         respective  body's  center  of  mass  (kg  mA2) 

%         1  st  element  =  inertia  of  spacecraft  centerbody 

%         2nd  &  3rd  elements  =  inertia  of  left  arm  (upper  aim  then  lower  aim) 

%         4th  &  5th  elements  =  inertia  of  right  arm  (upper  arm  then  lower  aim) 

%         6th  element  =  pavload  inertia 

%         [10;  IL 1 ;  IL2;  IR1 ;  IR2;  IP] 

%  Q  =  8x1  column  vector  of  generalized  coordinates 

%  Qdot  =  8x1  vector  of  generalized  velocities 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  CONVERT  INPUTS  FROM  ARRAYS  TO  SCALARS  %% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Lengths  (m) 

L0  =  Ls(l); 

LI  =Ls(2); 

L2  =  Ls(3); 

LP  =  Ls(4); 

R2  =  Ls(5); 

Rl  =Ls(6); 

R0  =  Ls(7); 

%  Member  masses  (kg) 
MO  =Ms(l); 
ML1  =Ms(2); 
ML2  =  Ms(3); 
MR1  =Ms(4); 
MR2  =  Ms(5); 
MP  =  Ms(6), 

%  Center  of  mass  distances  (m) 
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LcO  =CMs(l); 

LcLl  =  CMs(2); 

LcL2  =  CMs(3); 

LcRl  =CMs(4); 

LcR2  =  CMs(5); 

LcP  =  CMs(6);  %measured  from  left  end 

%  MOl  about  center  of  mass 

10  =  Is(l); 

IL 1  =  Is(2); 

IL2  =  Is(3); 

1R1  =Is(4); 

IR2  =  Is(5); 

IP  =  Is(6); 

%  Coordinates  (rad  &  m) 

ThO  =Q(1); 

ThLl  =  Q(2); 

ThL2  =  Q(3); 

ThR  1  =  Q(4). 

ThR2  =  Q(5); 

ThP  =  Q(6); 

XP  =Q(7); 

YP  =Q(8); 

%  Coordinate  Rates  (rad/sec  &  m/sec) 

ThOd  =Qdot(l); 

ThLld  =  Qdot(2); 

ThI.2d  =  Qdot(3); 

ThRld  =  Qdot(4); 

ThR2d  =  Qdot(5); 

ThPd  =  Qdot(6); 

XPd    =Qdot(7); 

YPd    =Qdot(8); 

%  Angular  Momentum 

HO  =  Th0d*(I0  +  MO*LcOA2); 

HLl=Th0d*(ILl+MLl*(L0A2+LcLlA2+2*L0*LeI,l*cos(ThLl )))  +  ... 

ThL  1  d*(IL  1 +ML 1  *(LcL  1  A2+L0*LcL  1  *cos(ThL  1 ))); 
HL2  =  Th0d*(IL2+ML2*(L0A2+L  1  A2+LcL2A2+2*LO*I ,  1  *cos(ThL  1 )  +  .. 
2*L  1  *LcL2*cos(ThL2)+2*L0*LcL2*cos(Thl  - 1  +ThL2)))  +  ... 
ThL  1  d*(IL2+ML2*(L  1  A2+LcL2A2+L()*L  1  *cos(ThL  1 )  + ... 
2*Ll*LcL2*cos(ThL2)+L0*LcL2*cos(ThI.l+TliI.2)))  +  ... 
ThL2d*(lL2+ML2*(LcL2A2+Ll*LcL2*eos(ThL2)  + ... 
L0*LcL2*cos(ThL  l+ThL2))); 
HR1  =Th0d*(IRl+MRl*(R0A2+LcRlA2+2*R0*I,cRl*cosfThRl )))  +  ... 

ThR  1  d*(IR  1 +MR 1  *(LcR  1  A2+R0*LcR  1  *cos(ThR  1 ))): 
HR2  =  Th0d*(IR2+MR2*(R0A2+R  1  A2+LcR2A2+2*RO*R  1  *cos(ThR  1 )  + 
2*Rl*LcR2*cos(ThR2)+2*R0*LcR2*cos(ThRI+ThR2)))  +  ... 
ThRld*(IR2+MR2*(RlA2+LcR2A2+RO*Rl*cos(ThRl)  + ... 
2*Rl*LcR2*cos(ThR2)+RO*LcR2*cos(ThRl+ThR2)))  + ... 
ThR2d*(IR2+MR2*(LcR2A2+Rl*LcR2*cos(ThR2)  +  ... 
R0*LcR2*cos(ThR  1  +ThR2))); 
HP  =  ThPd*IP  +  MP*(-XPd*YP  +  YPd*XP); 
HTotal  =  HO  +  HL 1  +  HL2  +  HR1  +  HR2  +  HP; 
Hs  =  [HO  HL1  HL2  HR1  HR2  HP  HTotal]; 
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B.  AngMo2 

%  Filename  is  "AngMo2.m" 

%  This  file  calculates  the  angular  momentum  of  the  system 
%  Version  2  also  finds  the  rate  of  change  of  angular  momentum 
function  [Hs,  Hdots]  =  AngMo2(Ls,Ms,CMs,Is,Q,Qdot.Qddot) 

%  OUTPUT: 

%  Hs  =  1x7  row  vector  of  angular  velocities.  The  first  element  is  for 

%  the  cenlerbody.  The  next  four  elements  are  for  the  left  upper 

%  and  lower  aim  followed  by  the  right  upper  and  lower  arm    The 

%  last  two  elements  are  for  the  payload  and  a  total  of  all  the 

%  previous  elements.  [HO  HL 1  HL2  HR1  1 1R2  I  IP  I  ITotal] 

%  Hdots  =  1x7  row  vector  of  the  change  in  angular  velocities.  7  he  order 

%  is  the  same  as  for  Hs 

% 

%  INPUT: 

%  Ls  =  7x1  vector  of  lengths  (m) 

%  1  st  element  =  distance  from  origin  to  left  arm  mount 

%  2nd  &  3rd  elements  wrt  left  arm  (from  shoulder  toward  wrist) 

%  4th  element  =  payload  length 

%  5th  &  6th  elements  wrt  right  arm  (from  wrist  toward  shoulder) 

%  7th  element  =  distance  from  right  arm  mount  to  origin 

%  [LO;  LI;  L2;  LP;  R2;  Rl;  ROJ 

%  Ms  =  6x1  column  vector  containing  the  masses  (kg) 

%  1  st  element  =  mass  of  spacecraft  cenlerbody 

%  2nd  &  3rd  elements  =  mass  of  left  arm  (upper  arm  then  lower  arm) 

%  4th  &  5th  elements  =  mass  of  right  arm  (upper  arm  then  lower  ami) 

%  6th  element  =  pa v load  mass 

%  [MO;  ML  1 ;  ML2;  MR  1 ;  MR2;  MP] 

%  CMs  =  6x1  column  vector  containing  center  of  mass  locations 

%      [LcO;  LcL  1 ;  LcL2;  LcR  1 ;  LcR2;  LcP] 

%  Is  =  6x1  column  vector  containing  the  moments  of  inertias  about  the 

%         respective  body's  center  of  mass  (kg  mA2) 

%         1  st  element  =  inertia  of  spacecraft  centerbody 

%         2nd  &  3rd  elements  =  inertia  of  left  arm  (upper  arm  then  lower  aim) 

%         4lh  &  5th  elements  =  inertia  of  right  arm  (upper  arm  then  lower  aim) 

%        6th  element  =  payload  inertia 

%         [I0;IL1;IL2;  IR1;  IR2;  IP] 

%  Q  =  8x  1  column  vector  of  generalized  coordinates 

%  Qdot  =  8x1  vector  of  generalized  velocities 

%  Qddot  =  8x1  vector  of  generalized  accelerations 

%%%%%%%%%%%%%%%%%%0/o%%%%%%%%0/i,%%% 

%%  CONVERT  INPUTS  FROM  ARRAYS  TO  SCALARS  %% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Lengths  (m) 

L0  =  Ls(l); 

LI  =Ls(2); 

L2  =  Ls(3); 

LP  =  Ls(4); 

R2  =  Ls(5); 

Rl  =Ls(6); 

RO  =  Ls(7); 

%  Member  masses  (kg) 
MO  =Ms(l); 
ML1  =Ms(2); 
ML2  =  Ms(3); 
MR1  =Ms(4); 


n: 


MR2  =  Ms(5); 
MP  =  Ms(6), 

%  Center  of  mass  distances  (m) 
LcO  =CMs(l); 
I.el.l  =CMs(2); 
LcL2  =  CMs(3); 

LcRl  =  CMs(4), 
LcR2  =  CMs(5); 
LcP  =  CMs(6);  %measured  from  left  end 

%  MOI  about  center  of  mass 

10  =Is(l); 

11,1  =Is(2); 

IL2  =  Is(3); 

IR1  =ls(4); 

IR2  -  Is(5); 

IP  =Is(6); 

%  Coordinates  (rad  &  m) 

ThO  =Q(1); 

ThL  1  =  Q(2); 

ThL2  =  Q(3); 

ThRl  =Q(4); 

ThR2  =  Q(5); 

ThP  =  Q(6); 

XP  =Q(7); 

YP  =Q(8); 

%  Coordinate  Rates  (rad/sec  &  m/sec) 

ThOd  =Qdot(l); 

ThLld  =  Qdot(2); 

ThL  2d  =  Qdot(3); 

ThRld  =  Qdot(4); 

ThR2d  -  Qdot(5); 

ThPd  =Qdot(6); 

XPd  =Qdot(7); 

YPd  =Qdot(8); 

%  Coordinate  Accelerations  (rad/secA2  &  m/secA2) 

ThOdd  =Qddot(l); 

ThLldd  =  Qddot(2); 

ThL2dd  =  Qddot(3); 

ThRldd  =  Qddot(4), 

ThR2dd  =  Qddol(5); 

ThPdd  =  Qddot(6), 

XPdd    =Qddot(7); 

YPdd    =Qddot(8), 

%  Angular  Momentum 

HO  =  Th0d*(I0  +  M0*Lc0A2), 

HL1  =  ThOd*(IL  1 +ML 1  *(L0A2+LcL  1  A2+2*L0*Ld .  I  *cos(ThL  1 )))  +  ... 

ThL  1  d*(IL  1 +ML 1  *(LcL  1  A2+L0*LcL  1  *cos(Thl ,  1 ))); 
HL2  =  Th0d*(IL2+ML2*a0A2+L  1  A2+LcL2A2+2*l  ,0*L  1  *cos(ThL  1 )  + 

2*Ll*LcL2*cos(ThL2)+2*L0*LcL2*cos(ThLl+ThL2)))  +  ... 

ThL  ld*(IL2+ML2*(L  1  A2+LcL2A2+LO*L  1  *cos(ThL  1 )  +  ... 

2*Ll*LcL2*cos(ThL2)+L0*LcL2*cos(Thl.l+ThL2)))  +  ... 

ThL2d*(IL2+ML2*(LcL2A2+Ll*LcL2*cos(ThL2)  +  ... 

L0*LcL2*cos(ThL  l+ThL2))); 
IIR1  =Th0d*(IRl+MRl*(R0A2+LcRlA2+2*R0*LcRl*cos(ThRl)))  +  .. 

ThRld*(lRl+MRl*(LcRlA2+R0*LcRl*cos(ThRl))): 
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HR2  =  ThOd*(IR2+MR2*(ROA2+RlA2+LcR2A2+2*RO*Rl*cos(ThRl)  +  ... 
2*Rl*LcR2*cos(ThR2)+2*R0*LcR2*cos(ThRl+ThR2)))  + ... 

ThRld*(IR2+MR2*(RlA2+LcR2A2+RO*Rl*cos(ThRl)  +  ... 

2*Rl*LcR2*cos(ThR2)+R0*LcR2*cos(ThRI+ThR2)))  + ... 

ThR2d*(IR2+MR2*(LcR2A2+Rl*LcR2*cos(ThR2)  +  ... 

R0*LcR2*cos(ThR  1  +ThR2))); 
I  IP  =  ThPd*IP  +  MP*(-XPd*  YP  +  YPd*XP); 
HTotal  =  HO  +  HL 1  +  ML  2  +  HR1  +  HR2  +  HP, 
Hs  =  [HO  HL1  HL2  HR1  HR2  HP  HTotal], 

%  Change  in  angular  momentum 
HOd  =  Th0dd*(10  +  M0*Lc0A2), 

HL  Id  =  ThOdd*(ILl+MLl  *(L0A2+LcLlA2+2*L0*LcLl*cos(ThLl )))  +  .. . 
ThL  1  dd*(IL  1 +ML 1  *(LcL  1  A2+L0*LcI .  1  *cos(ThL  1 )))-.. . 
ThOd*ThI >  ld*2*ML  1  *LO*LcL  1  *sin(ThL  1 )  - ... 
ThL  1  dA2*ML  1  *L0*LcL  1  *sin(ThL  1 ). 
HL2d  =  ThOdd*(IL2+ML2*(LOA2+Ll  A2+LcL2A2+2*LO*L  1  *cos(ThL  1)  +  ... 
2*Ll*LcL2*cos(ThL2)+2*L0*LcL2*cos(ThLl+ThL2)))  + ... 
ThLldd*(IL2+ML2*(LlA2+LcL2A2+L0*Ll*cos(ThLl)  + ... 
2*Ll*LcL2*cos(ThL2)+L0*LcL2*cos(ThLl+ThL2)))  +  ... 
ThL2dd*(IL2+ML2*(LcL2A2+Ll*LcL2*cos(ThL2)  + ... 
L0*I.cL2*cos(ThL1+ThL2)))  - ... 

Th0d*ThLld*2*ML2*(L0*Ll*sin(ThLl)+L0*LcL2*sin(ThLl+ThL2))-... 
ThOd*ThL2d*2*ML2*(Ll*LcL2*sin(ThL2)+LO*LcL2*sin(ThI.l+Thl2))-... 
ThLld*ThL2d*2*ML2*(Ll*LcL2*sinfThL2)+L0*LcL2*sin(ThLl+ThL2))-... 
ThL  1  dA2*ML2*(L0*L  1  *sin(ThL  1  )+L0*LcL2*sin(ThL  1 +ThL2))  - ... 
ThL2dA2*ML2*(L  1  *LcL2*sin(ThL2)+I.O*LcL2*sin(ThL  1  +ThL2)); 
HRld  =  Th0dd*(IRl+MRl*(R0A2+LcRlA2+2*R()*I.cRl*cos(ThRl )))  +  ... 
ThRldd*(IRl+MRl*(LcRlA2+R0*LcRl*cos(ThRl)))-... 
ThOd*ThR  1  d*2*MR  1  *R0*LcR  1  *sin(ThR  1 )  -  ... 
ThR  1  dA2*MR  1  *R0*LcR  1  *sin(ThR  1 ); 
HR2d  =  ThOdd*(IR2+MR2*(ROA2+RlA2+LcR2A2+2*RO*Rl*cos(ThRl)  +  ... 
2*Rl*LcR2*cos(ThR2)+2*RO*LcR2*cos(ThRl+ThR2)))  + ... 
ThRlddW2+MR2*(RlA2+LcR2A2+RO*Rl*cosfThRl)  + ... 
2*Rl*LcR2*cos(ThR2)+R0*LcR2*cos(ThRl+ThR2)))  + ... 
ThR2dd*(IR2+MR2*(LcR2A2+Rl*LcR2*cos(ThR2)  + ... 
R0*LcR2*cos(ThRl+ThR2)))  - ... 

Th0d*ThRld*2*MR2*(R0*Rl*sin(ThRl)+R0*LcR2*sin(ThRl+ThR2))  - ... 
Th0d*ThR2d*2*MR2*(Rl*LcR2*sin(ThR2)+R0*LcR2*sin(ThRl+ThR2))-... 
ThRld*ThR2d*2*MR2*(Rl*LcR2*sin(ThR2)+RO*LcR2*sin(ThRl+ThR2))- 
ThR  1  dA2*MR2*(R0*R  1  *sin(ThR  1  )+R0*LcR2*sin(ThR  1  +ThR2))  - . .. 
ThR2dA2*MR2*(Rl*LcR2*sin(ThR2)+RO*LcR2*sin(ThRl+ThR2)); 
HPd  =  ThPdd*IP  +  MP*(-XPdd*YP  -  XPd*YPd  +  YPdd*XP  +  YPd*XPd); 
HdTotal  =  HOd  +  HL  1  d  +  HL2d  +  HR 1  d  +  HR2d  +  HPd. 
Hdots  =  [HOd  HLld  HL2d  HRld  HR2d  HPd  HdTotal  J; 


C.  Draw3 

%  Filename  is  'Draw3.m' 

function[X,Y]  =  Dra\v3(Lengths,AngConst,AngHist. Interval) 

%%%%%%%%%%%%%%0/o%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%  This  file  draws  the  dual  arm  spacecraft  stick  figure 

% 

%  INPUTS: 

% 

%  Lengths  =  7x1  vector  of  link  lengths  (m) 
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%  1st  element  is  distance  from  origin  to  left  arm  mount 

%  2nd  &  3rd  elements  wrt  left  arm  (from  shoulder  toward  wrist) 

%  4th  element  is  pay  load  length 

%  5th  &  6th  elements  wrt  right  arm  (from  wrist  toward  shoulder) 

%  7th  element  is  distance  from  right  ami  mount  to  origin 

%  AngConst  =  vector  of  angles  to  ami  mounting  locations  wrt  centcrhody  coord 

%  frame  (angle  for  left  ami  then  angle  for  right  ami) 

%  AngHist  =  n\6  matrix  of  angle  histories    Each  row  represents  a 

%  specific  time.  Kach  column  represents  a  specific  joint 

%  angle  (except  the  pay  load  angle  is  inertial) 

%  1st  column  is  the  center  body  angle 

%  2nd  &  3rd  columns  are  the  left  arm  shoulder  and  elbow 

%  4th  &  5th  columns  are  the  right  arm  shoulder  and  elbow 

%  6th  column  is  the  pay  load  (this  angle  is  inertial) 

%  Interval  =  plot  every  "interval'th"  time 

% 

% 

%  OUTPUTS: 

% 

%  X  =  vector  history  of  joint  x  coordinates 

%  Y  =  vector  history  of  joint  y  coordinates 

%  X  &  Y  treat  the  system  as  a  closed  chain  beginning  at  the  centerbody  origin, 

%  outward  along  the  left  arm,  across  the  pay  load,  inward  along  the  right  arm, 

%  and  back  to  the  origin. 

% 

%%%%%%%%%%%%%%%%%%%%%%%%%%,M,%%%%%%%%%%%%%% 

[Times,dummy]  =  size(  AngHist); 

Links  =  length(Lengths); 

X(1,1)  =  0; 

Y(1,1)  =  0; 

%  Convert  the  joint  angles  to  inertial  angles  and  reorder  them  for  closed  chain  use 


NAng( 
NAng( 
NAng( 
NAng( 
NAng( 
NAng( 
NAng( 


)  =  AngHist(:,l)  +  AngConst(l)*ones(Times.l ); 
,2)  =  NAng(:,l )  +  AngUist(:,2); 
,3)  =  NAng(:,2)  +  AngHist(:,3); 
,4)  =  AngHist(:,6); 

,7)  =  AngUist(:,l)  +  AngConst(2)*ones(Times,l)  +  pi; 
,6)  =  NAng(:,7)  +  AngHist(:,4); 
,5)  =  NAng(:,6)  +  AngHist(:,5); 


p=l; 

while  p  <=  Times 

for  q  =  l:Links 

Lastx  =  0; 

Lasty  =  0; 

for  r  =  1  :q 

Lastx  =  Lastx  +  Lengths(r)*cos(NAng(p,r)); 
Lasty  =  Lasty  +  Lenglhs(r)*sin(NAng(p,r)); 
end 

X(q+l,p)  =  Lastx; 
Y(q+l,p)  =  Lasty; 
end 

p  =  p  +  Interval; 
end 

X=  [X(l:Links,:);  X(2,;);  X(Links,:);  X(Links+l,:)J, 
Y  =  [Y(l:Links,:);  Y(2,:);  Y(Links,:);  Y(Links+l,:)]; 

%  Plot  the  Final  Case 
for  q=  LLinks 

Lastx  =  0; 

Lasty  =  0; 
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for  r  =  1  :q 

Lastx  =  Lastx  +  Lengths(r)*cos(NAng(Times,r)); 
Lasty  =  Lasty  +  Lengths(r)*sin(NAng(Times,r)); 
end 

XFinaI(q+l,l)  =  Lastx; 
YFinal(q+l,l)  =  Lasty; 
end 

XFinal  =  fXFinal(l:Links,:);  XFinal(2,:);  XFinalfLinks,:);  XFinal(Links+l,:)]; 
YFinal  =  [YFinal(l:Links,:);  YFinal(2,:);  YFmal(Links,:);  YFinal(Links+l,:)]; 

clg; 

axis('square') 

plot(X,Y;-,,XFinal.YFinai:-',XFinal,YFinal,V,  X(:,I),Y(:,1),V); 

xlabel('X  (m)');ylabel('Y  (m)'); 

axis('normal') 


D.  Eqn2 


%  Filename  is  'Eqn2.m' 

%  Differential  Equations  for  numerical  integrator 

function  [Xdot,U.TorqRef,Aqdot,J,Res,LHS,RHS.Delql  =... 

Eqn2(T,X,Ls,Ms,CMs,Is,BoundC,Gains,XfDes.Wu.Wc,Coef,ConslMat) 

%  OUTPUT: 

%  xdot  =  derivatives  of  state  vector  at  time  T 

%  U  =  column  vector  of  actual  torques  commanded  at  time  T  arranged 

%         as  [U 1 ;  U2;  U6;  U5]  where  the  number  denotes  the  joint 

%         associated  with  that  torque 

%  TorqRef  =  column  \     tor  of  reference  torques  that  should  be  applied 

%         at  time  T  if  the  motion  followed  the  reference  maneuver  exactly 

%         These  are  arranged  in  the  same  order  as  U. 

%  Res  =  column  vector  of  residuals  after  EOM  are  evaluated  with  the 

%        calculated  reference  torques    (Residuals  should  be  zero). 

%  Aqdot  =  column  vector  of  A*qdot.  This  is  a  test  to  see  if  the 

%        constraint  equation  (A*qdot  =  0)  is  satisfied 

%  LHS  -  column  vector  of  the  EOM  left  hand  side  (1.1  IS  =  M*qddot  +  C.Tilda) 

%  RI  IS  =  column  vector  of  the  EOM  right  hand  side  (Rl  IS  =  BTilda*u) 

% 

%  INPUTS: 

%  T  =  time  (sec) 

%  State  Vector,  X,  is  defined  as  follows: 

%       XI  =ThetaO(rad) 

%       X2  =ThetaLl  (rad) 

%      X3  =  Theta  L2  (rad) 

%       X4  =  Theta  Rl  (rad) 

%       X5  =  Theta  R2  (rad) 

%       X6  =  Theta  P  (rad) 

%       X7  =  X  component  of  Payload  Center  of  mass  position  (m) 

%       X8  =  Y  component  of  Payload  Center  of  mass  position  (m) 

%       X9  =  Theta  0  Dot  (rad/sec) 

%       X 1 0  =  Theta  L 1  Dot  (rad/sec) 

%       X 1 1  =  Theta  L2  Dot  (rad/sec) 

%      X 1 2  =  Theta  R 1  Dot  (rad/sec) 

%       X 1 3  =  Theta  R2  Dot  (rad/sec) 

%       X 1 4  =  Theta  P  Dot  (rad/sec) 

%       X 1 5  =  X  component  of  Payload  Center  of  mass  velocity  (m/sec) 

%       X16  =  Y  component  of  Payload  Center  of  mass  velocity  (m/sec) 

%       X 1 7  =  integral  of  the  absolute  value  of  the  centerbody  disturbance  torque 

%       XI 8  =  integral  of  the  centerbody  disturbance  torque  squared 
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%  Ls  =  7x1  vector  of  lengths  (m) 

%  1st  element  =  distance  from  ongin  to  left  ami  mount 

%  2nd  &  3rd  elements  vvrt  left  arm  (from  shoulder  toward  wrist) 

%  4th  element  =  pay  load  length 

%  5th  &  6lh  elements  wrt  right  arm  (from  wrist  toward  shoulder) 

%  7th  element  =  distance  from  right  ami  mount  to  origin 

%  [L0;L1;L2;  LP;  R2;  Rl;  R0[ 

%  Ms  =  6x1  column  vector  containing  the  masses  (kg) 

%  1  st  element  =  mass  of  spacecraft  centerhody 

%  2nd  &  3rd  elements  =  mass  of  left  ami  (upper  ami  then  lower  arm) 

%  4th  &  5th  elements  =  mass  of  right  ann  (upper  ami  then  lower  ami) 

%       6th  element  =  pavload  mass 

%       [MO;  ML  1 ;  ML2;  MR  1 ;  MR2;  MP] 

%  CMs  =  6x1  column  vector  containing  center  of  mass  locations 

%   [LcO;  LcL  1 ;  LcL2;  LcR  1 ;  LcR2;  LcP] 

%  Is  =  6x1  column  vector  containing  the  moments  of  inertias  about  the 

%       respective  body's  center  of  mass  (kg  mA2) 

%       1  st  element  =  inertia  of  spacecraft  centerhody 

%       2nd  &  3rd  elements  =  inertia  of  left  arm  (upper  arm  then  lower  ami) 

%       4th  &  5th  elements  =  inertia  of  right  arm  (upper  arm  then  lower  arm) 

%       6th  element  =  payload  inertia 

%       [10;  IL1;  IL2;  IR1;  IR2;  IP] 

%  BoundC  =  boundry  conditions  for  the  problem.  The  first  column 

%       contains  the  initial  x  and  y  component  of  points  Q  &  P 

%       respectively,  the  x  component  of  the  right  arm  base,  the 

%       problem  start  time,  and  the  simulation  stop  time.  The  second 

%       column  contains  the  x  and  y  component  of  points  Q  &  P 

%       respectively,  the  x  component  of  the  right  arm  base,  the 

%       stop  time  for  the  ideal  reference  maneuver,  and  a  Hag  to 

%       activate  or  deactivate  the  controller.  The  origin  for  the 

%       x  and  y  components  is  the  base  of  the  left  arm. 

%  Wu  =  6x6  control  torque  cost  weighting  matrix 

%  Wc  =  8x8  constraint  cost  weighting  matrix 

%  Gains  =  1  x2  column  vector  of  controller  gains.  The  first  value  is 

%       for  position  gains  and  the  second  value  is  for  velocity 

%       gains. 

%  XfDes  =  column  vector  containing  desired  values  for  the  angles  at 

%       the  conclusion  of  the  maneuver.  These  are  the  same  angles 

%       the  reference  maneuver  is  trying  to  create.  Thev  are  arranged 

%       as  [ThOf;  ThL If;  ThL2f;  ThR  1  f;  ThR2f;  ThPf].' 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  CONVERT  INPUTS  FROM  ARRAYS  TO  SCALARS  %% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

ThO  =X(1); 

ThL  1  =  X(2); 

ThL  2  =  X(3); 

ThRl  =X(4); 

ThR2  =  X(5); 

ThP  =  X(6); 

Xc    =X(7); 

Yc    =X(8); 

ThOd  =X(9); 

ThLld  =  X(10); 

ThL2d  =  X(ll); 

ThRld  =  X(12); 

ThR2d  =  X(13), 

ThPd  =X(14); 

Xcd    =X(15); 

Ycd    =X(16); 
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%  Arms  mount  locations  wrt  spacecraft  centerbodv  coordinate  frame  (rad) 
ThLO  =  BoundC(5,l);  ThRO  =  BoundC(5,2); 

%  Stop  Times 

TfR  =  BoundC(6,2);      %  Reference  Torque  stop  time  (sec) 

TfC  =  BoundC(7,l );      %  Controller  stop  time  (sec) 

%  Controller  Flag 
ContFlag  =  BoundC(7,2); 

%  Constraints  Matrix  Flag 
AMatFlag  =  BoundC(8,l), 

%  Centerbody  Reaction  Wheel  Flag 
WheelFlag  =  BoundC(8,2); 

%  Kinetic  Energy  Test  Flag 
KEFlag  =  BoundC(ll,l); 

%  Inverse  Kinematics  Bypass  Flag 
ByPass  =  BoundC(ll,2); 

%  Torque  selection  if  bypass  is  enabled 
TorqFlag  =  BoundC(12,l); 

%  Maximum  torque  from  reaction  wheel 
TorqCap  =  BoundC(13,l),      %  Limit  enabled 
TorqMax  =  BoundC(13,2);      %  Limit  amount 

%  Controller  Gains: 
Gpos  =  Gains(l); 
Gvel  =  Gains(2); 

%%%%%%%%%%%%%% 

%%  CALCULATIONS  %% 

%%%%%%%%%%%%%% 

%  ROM:  M*qddot  +  dV/dq  +  G  -  Qf  +  A'*Lam 

%  M  is  mass  matrix 

%  qddot  is  column  vector  of  generalized  coordinate  accelerations 

%  dV/dq  is  the  partial  derivative  of  the  potential  function  with 

%       respect  to  the  generalized  coordinates    This  term  is  zero  for 

%      this  problem  because  all  motion  is  in  the  horizontal  plane  (there 

%       is  no  change  in  potential  energy  caused  by  the  motion) 

%  G  is  a  column  vector  which  is  a  function  of  q  and  qdot 

%  Qf  are  generalized  forces  caused  by  joint  torques 

%  A'  is  transpose  of  constraints  matrix 

%  Lam  are  Lagrange  multipliers 

%%%%%%%%%%%%%%%%%% 

%%  State  Vector  &  Derivative  %% 

%%%%%%%%%%%%%%%%%% 

Q    =[ThO;  ThLh  ThL2;  ThRL.  ThR2;  ThP;  Xc;  Yc]; 

Qdot  =  [ThOd;  ThLld;  ThL2d;  ThRld;  TliR2d;  ThPd;  Xcd.  Ycd]; 

%%%%%%%%% 
%%  Matrices  %% 
%%%%%%%%% 
AngConst  -  [ThLO;  ThRO]. 
if  AMatFlag 

[M,G,A,Adot,B]  =  MatxFix(Ls,Ms,CMs,Is,Q,Qdot,AngConst); 
else 
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|M,G,A,Adot,BJ  =  Matx(Ls,Ms,CMs,Is,Q,Qdot,AngConst); 

end 

ifWheelFlag 

B7  =  [l;0;0;0;0;0;0;0]; 

B  =  [B7  B], 
end 

if  ByPass    %  If  true,  then  bypass  calculating  torques  using  inverse 
%  kinematics.  This  branch  of  logic  is  a  verification  test 
%  during  program  development  and  is  not  intended  for  regular 
%  use  once  the  program  is  checked  out. 
if  TorqFlag  ==  0 

U  =  zeros(6,l);    J  =  0; 
else 

if  TorqFlag  ==  1 

U  =  [-0.01;0;0;0;0;0];  .1  =  0; 
else 

U  =  [-0.0 1;0;0;  0.01  ;0;0];  J  =  0; 
end 
end 
if  WheelFlag 

U  =  [0;  U]; 
end 
else         %  Normal  program  flow  to  find  control  torques 
%%%%%%%%% 
%%  Torques  %% 
%%%%%%%%% 
if  T  <=  TfR,     %  Get  the  appropriate  torque  and  angle  values 
%  from  the  reference  maneuver  calculations 
[TorqRef,  QRef,  QdotRef,  Aqdot,  J,  CIRef,  C2Ref,  C3Ref]  =  ... 

Ref2(Ls,Ms,CMs,Is,BoundC,T,Wu,Wc,Coef,ConstMat); 
else  %  Simulation  is  longer  than  ideal  reference  maneuver 

%  Use  no  reference  torques 
%  Use  the  desired  final  values  as  references 
TorqRef  =  [0;  0;  0;  0;  0;  0]; 
QRef(l)     =XfDes(l) 
QRef(2)     =  XfDes(2) 
QRef(3)     =  XfDes(3) 
QRef(4)     =  XfDes(4) 
QRef(5)     =  XfDes(5) 
QRef(6)     =  XfDes(6) 
QRef(7)     =  XfDes(7) 
QRef(8)     =  XfDes(8) 
QdotRef(l)  =  XfDes(9); 
QdotRef(2)  =  XfDes(10) 
QdotRef(3)  =  XfDes(ll) 
QdotRef(4)  =  XfDes(12) 
QdotRef(5)  =  XfDes(13) 
QdotRef(6)  =  XfDes(14) 
QdotRef(7)  =  XfDes(15) 
QdotRef(8)  =  XfDes(16) 
if  WheelFlag 

TorqRef  =[0;  TorqRef]; 
end 

%  Matrices 
if  AMatFlag 

[MRef,GRef,ARef,AdotRef]  =  Mat\Fi\(Ls.Ms,CMs,Is,... 

QRef,QdotRef,AngConst); 
else 
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[MRef,GRef,ARef,AdotRef]  =  Matx(Ls.Ms.CMs,Is,QRef,QdotRef, ... 

AngConst); 
end 

BRef=B; 

Pt  1  Ref  =  ARer*inv(ARef*inv(MReO*ARef); 
CIRef  =  inv(MRef)*(eve(M)  -  PtlRef*ARef*inv(MRef))*BRef, 
C2Ref  =  -mv(MRef)*Pt  1  Ref*AdotRef; 

C3Ref  =  inv(MRef)*(PtlRef*ARef*inv(MRef)  -  eye(M))*GRef; 
end 

if  ContFlag         %  Controller  is  on 
Delq  -  Q  -  QRef ; 
Delqdot  =  Qdot  -  QdotRef ; 
%  Controller  caleulalions 
Ptl  =  A'*inv(A*inv(M)*A'); 
CI  =  inv(M)*(eve(M)  -  Ptl*A*inv(M))*B; 
C2  =  -inv(M)*P'tl*Adot; 
C3  =  inv(M)*(Ptl*A*inv(M)  -  eye(M))*G; 
F2  =  Gpos  *  Delq; 
F2  =  [F2(1:6);0;0]; 
Kv  =  Gvel  *  eye(M); 
Kv(7,7)=0;  Kv(8,8)=0; 
Pt3  =  pinv(Cl); 
%  Pt3  =  uiv(Cr*Cl)*Cl';  %  Resulted  in  poorly  conditioned  matrix 

%%%%%%%%%%%%%%%%%%%% 
%%  Complete  Lyapunov  Controller  %% 
%%%%%%%%%%%%%%%%%%%% 

U  -  Pt3*(-Kv*Delqdot  +  ClRef*TorqRcf  -  (C2*Qdot  -  C2Ref*QdotRcD 
(C3  -  C3Ref)  -  F2); 
%%%%%%%%%%%%%%%%%%%%%% 
%%  Simplified  Lyapunov  Controller        %% 
%%  (removes  reference  torques  and  %% 

%%  assumes  C2  and  C3  terms  are  small)  %% 
%%%%%%%%%%%%%%%%%%%%%% 
%  Kp  =  Gpos  *  eye(M); 

%  Pt3  =  pinv(ClReO; 

%  U  =  Pt3*(-(Kv+C2ReO*Delqdot  -  Kp*Delq)  +  TorqRef; 

else  %  Controller  is  off 

U  =  TorqRef;  %  Don't  adjust  torques  from  reference  maneuver 

Delq  =  999*ones(8. 1 );  %  Dummy  value  for  trajectory  error 
end  %  End  of  Control  Loop 

if  WheelFlag 

J  =  abs(U(l)); 
else 

J  =  0; 
end 
end  %  End  of  Kinetic  Energy  Test  Conditional 

if  TorqCap       %  Upper  limit  on  wheel  torque  enabled? 
ifabs(U(l))>TorqMax 
ifU(l)>0 

U(l)  =  TorqMax; 
else 

U(l)  =  -TorqMax; 
end 
end 
end 

%%%%%%% 
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%%  Qf   %% 

%%%%%%% 

%  Qf  =  B*u    These  are  tlie  generalized  forces 

Qf=B*U; 

%%%%%%%%%%%%%%% 

%%  Lagrange  Multipliers  %% 

%%%%%%%%%%%%%%% 

%  EOM:  M*qddot  +  dV/dq  +  G  =  Qf  +  A'*Lam 

%  Solving  the  EOM  for  qddot  gives:  qddot  =  inv(M)*(Qf  +  A'*Lam  -  G) 

%  Differentiating  the  Pfaffian  form  of  the  constraint  equations 

%  results  in:  Adot*qdot  +  A*qddot  =  0. 

%  Substitution  of  the  expression  for  qddot  into  the  previous  equation 

%  permits  solving  for  Lam: 

%        Lam  =  inv(A*inv(M)*A')*(A*inv(M)*(G-Qf)  -  Adot*qdot); 

Lam  =  inv(A*inv(M)*A')*(A*inv(M)*(G-Qf)  -  Adot*Qdot); 

%%%%%%%%%%%%%%% 
%%  Putting  it  all  together  %% 
%%%%%%%%%%%%%%% 
Qddot  =  inv(M)  *  (Qf  +  A'*Lam  -  G), 

[lis,  Hdots]  -  AngMo2(Ls,Ms,CMs,Is,Q,Qdot,Qddot); 
%  Change  in  total  angular  momentum 
Hd  =  Hdots(7); 
•I  =  [J;  Hd]; 

%  Assemble  derivative  of  state  vector  for  integrator 
Xdot  =  [Qdot;  Qddot;  J(l);  (J(1))A2]; 

%%%%%%%%%%%%%%% 

%%  Troubleshooting  Info  %% 

%%%%%%%%%%%%%%% 

Aqdot  =  A*Qdot; 

LHS  -  M*Qddot  +  G; 

RHS  =  Qf  +  A'*Lam; 

Res  =  LHS  -  RHS. 


E.  fminu2 

function  [x,OPTIONS]  =  fminu2(FUN,x)OPTIONS,GI^ADFUN,Pl,P2.P3,P4,P5,P6,... 

P7,P8,P9,P10) 
%FMINU  Finds  the  minimum  of  a  function  of  several  variables. 
%        X=FMINU('FUN',X0)  starts  at  the  matrix  X()  and  finds  a  minimum  to  the 
%        function  which  is  described  in  FUN  (usually  an  M-file:  FUN.M) 
%        The  function  'FUN'  should  return  a  scalar  function  value:  F=FUN(X). 
% 

%        X=FMINU('FUN',X0,OPTIONS)  allows  a  vector  of  optional  parameters  to 
%        be  defined.  OPTIONS(l)  controls  how  much  display  output  is  given;  set 
%        to  1  for  a  tabular  display  of  results,  (default  is  no  display:  0). 
%        OPTIONS(2)  is  a  measure  of  the  precision  required  for  the  values  of 
%        X  at  the  solution  OPTIONS(3)  is  a  measure  of  the  precision 
%        required  of  the  objective  function  at  the  solution. 
%        For  more  information  type  HELP  FOPTIONS 
% 

%       X=FMINU('FUN',X0,OPTIONS,'GRADFUN')  enables  a  function'GRADFUN' 
%        to  be  entered  which  returns  the  partial  derivatives  of  the  function, 
%        df/dX,  at  the  point  X:  gf  =  GRADFUN(X). 
% 
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%  The  default  algorithm  is  the  BFGS  Quasi-Newton  method  with  a 

%  mixed  quadratic  and  cubic  line  search  procedure. 

%  Copyright  (c)  1990  by  the  Math  Works,  Inc. 

%  Andy  Grace  7-9-90. 


%  — Initialization- 

XOUT=x(:); 

nvars=length(XOUT); 


evalstr  =  [FUN]; 
if~any(FUN<48) 

evalstr=[evalstr,  '(x']; 
for  i=l:nargin  -  4 

evalstr  =  [evalstr,',P',num2str(i)]; 
end 

evalstr  =  [evalsu\ ')']; 
end 

if  nargin  <  3.  OPTIONS=f];  end 
if  nargin  <  4,  GRADFUN=[];  end 


if  length(GRADFT  IN) 
evalstr2  =  [GRADFUNJ; 
if~any(GRADFUN<48) 

evalstr2  =  [evalstr2,  '(x'J; 

for  i=l  margin  -  4 

evalstr2  =  [evalstr2,',P',num2str(i)]; 

end 

evalstr2  =  [evalstr2, ')']; 
end 
end 

f  =  eval(evalstr); 

n  =  length(XOUT); 

(JRAD=zeros(nvars,  1 ); 

OLDX=XOUT; 

MATX=zeros(3,l); 

MATL=[f;0,0J; 

OLDF=f; 

FIRSTF=f; 

[OLDX,OLDF,HESS,OFTIONS]=optint(XOUT,f.OPTIONS)-. 

CHG  =  le-7*abs(XOUT)+le-7*ones(nvars,l); 

SD  =  zeros(nvais,  1 ), 

diff  =  zeros(nvars,  1 ); 


OPTIONS(10)=2;  %  Iteration  count  (add  1  for  last  evaluation) 
status  =- 1 ; 

while  status  ~=  1 

%  Work  Out  Gradients 

if  -length(GRADFUN)  |  OPTIONS(9) 

OLDF=f; 
%  Finite  difference  perturbation  levels 
%  First  check  perturbation  level  is  not  less  than  search  direction. 

f  =  find(10*abs(CHG)>abs(SD)); 

CHG(f)  =  -0.1*SD(f); 
%  Ensure  within  user-defined  limits 
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CHG  =  sign(CHG+eps).*min(max(abs(CHG),OPTIONS(16)),OPTIONS(17)); 

for  gcnt=l:nvars 

XOUT(gcnt,  1  )=XOUT(gcnt)+CI  IG(gcn1 ); 
OPTIONS!  1 0)=OPTIONS(  1 0)+ 1 ; 
disp('While  Loop  Iteration  in  Progress'); 
disp(| 'Iterations:  ',num2str(OPTIONS(10))]); 
disp(['Allowable:    \num2str(OPTIONS(14))J). 
x(:)  =  XOUT;  f  =  eval(evalstr); 
GRAD(gcnt)=(f-OLDF)/(CHG(gcnt)); 
iff<OLDF 

OLDF=f; 
else 

XOUT(gcnt)=XOUT(gcnt)-CHG(gcnt); 
end 
end 
%  Try  to  set  difference  to  le-8  for  next  iteration 
CHG=  le-8./GRAD; 
f=OLDF; 
%    OPTIONS(10)=OPTIONS(10)+nvars; 
%  Gradient  check 

ifOPTIONS(9)==  1 
GRADFD  - GRAD; 
x(:)=XOUT;  GRAD  =  eval(evalstr2); 
graderr(GRADFD,  GRAD,  evalstr2); 
OPTIONS(9)  =  0; 
end 

else 

OPTIONS(ll)=OPTIONS(ll)+l; 

x(:)=XOUT;  GRAD  =  eval(evalstr2); 
end 
% — Initialization  of  Search  Direction 

if  status  ==  -1 
SD=-GRAD; 
FIRSTF=f; 
OLDG=GRAD; 

GDOLD=GRAD'*SD; 

%  For  initial  step-size  guess  assume  the  minimum  is  at  zero. 

OPTIONS(18)  =  max(0.01,  mm([l,2*abs(f/GDOLD)])), 

ifOPTIONS(l)>0 

%  disp([sprintf('%5.0f  %12.3g  %12.3g  ',OPfIONS(10),f,.. 

OPT10NS(  1 8)),spnntf('%  1 2.3g  ',GDOLD)J); 
end 

XOUT=XOUT+OPTIONS(  1 8)*SD; 
status=4; 
if  OPTIONS(7)==0;  PCNT=1 ;  end 

else 

%— Direction  Update — 

gdnew=GRAD'*SD; 
ifOPTIONS(l)X), 

num=[sprintf('%5.0f%12.3g%12.3g,,OFTlONS(10),f,OPTIONS(18)),... 
sprintf('%12.3g  ',gdnew)]; 
end 

if  (gdne\v>0  &  f>FIRSTF)|~finite(0 

%  Case  1 :  New  function  is  bigger  than  last  and  gradient  w.r.t.  SD  -ve 
%        ...interpolate. 

how-inter'; 

[stepsize  J-cubici  1  (f,FIRSTF,gdne\v,GDOLD,OPTIONS(  1 8)); 

if  stepsize<0|isnan(stepsize),  stepsize=OPTIONS(  1 8)/2;  how='C  1  f '.  end 

if  OPTIONS(  1 8)<0. 1  &OPriONS(6)==0 
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if  stepsize*norm(SD)<eps 
randOnormal') 
stepsize=rand(l); 

how='RANDOM  STEPLENGTII'; 
status=0; 
else 

stepsize=stepsize/2; 
end 
end 

OPTIONS(18)=stepsize; 
XOUT=OLDX; 
elseif  f<FIRSTF 

[newstep,fbest]  =cubici3(f,FIRSTF,gdnew,GDOLD,OPTIONS(  1 8)); 
sk=(XOUT-OLDX)'*(GRAD-OLDG); 
ifsk>le-20 
%  Case  2:  New  function  less  than  old  fun.  and  OK  lor  updating  HESS 
%  ....  update  and  calculate  new  direction 

how="; 

if  gdnewO 

how-incstep'; 

if  newstep<OPTIONS(  1 8) 

newstep=2*OPriONS(  1 8)+ 1  e-5 . 
how=[how,'  IF']; 
end 

OPTIONS(  1 8)=min([max([  2, 1 .5*OPTIONS(  1 8)] ),  1  +sk+ahs(  gdnew)+.. . 
max([0,bPTIONS(18>l]),(1.2-K).3*(~OPTIONS(7)))*abs(newstep)]); 
else  %  gdnew>0 

ifOPTIONS(18)>0.9 
how='int_st'; 

OPTIONS(18)=min([l,abs(newstep)]); 
end 
end  %if  gdnew 

[HESS,SDj=updhess(XOUT,OLDX,GRAD,OLDG,HESS.OPTIONS); 
gdnew=GRAD'*SD; 
OLDX=XOUT; 
status=4; 
%  Save  Variables  for  next  update 
FIRSTF=f; 
OLDG=GRAD; 
GDOLD=gdnew; 
%  If  mixed  interpolation  set  PCNT 

if  OPTIONS(7)=0,  PCNT=1 ;  MATX=zeros(3, 1 ).  MATL(  1  )=f;  end 
elseif  gdne\v>0  %sk<=0 

%  Case  3:  No  good  for  updating  HESSIAN  .  interpolate  or  halve  step  length, 
how-interst'; 
ifOPTIONS(18)>0.01 

OPTIONS(  1 8)=0.9*newstep; 
XOUT=OLDX; 
end 

if  OPTIONS(  1 8)>  1 ,  OPTIONS(  1 8)=  1 .  end 
else 
%  Increase  step,  replace  starting  point 

OPTIONS(18)=max(lmin(lnewstep-OFI'IONS(18)?31),0.5*OPTIONS(18)]); 
how-incst2', 
OLDX=XOUT; 
FIRSTF=f, 
OLDG=GRAD; 
GDOLD=GRAD'*SD; 
OLDX=XOUT; 
end  %  if  sk> 
%  Case  4:  New  function  bigger  than  old  but  gradient  in  on 
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%  ...reduce  step  length, 

else  %gdne\v<0  &  F>FIRSTF 
ifgdnew<0&tf>FIRSTF 
how-redstep'; 

if  norm(GRAD-OLDG)<le-10;  HESS=eye(nvars);  end 
ifabs(OPT!ONS(18))<eps 
rand('normal') 
SD=norm(SD)*rand(SD) 
OPTIONS(18)=abs(rand(l))*le-6; 
how='RANDOM  SD'; 
else 

OPTIONS(  1 8)=-OPTIONS(  1 8)/2; 
end 

XOUT=OLDX; 
end  %gdnew>0 
end  %  if  (gdnc\v>0  &  F>FIRSTF)|~finite(F) 
XOUT=XOUT+OPTIONS(  1 8)*SD; 
ifOPTIONS(l)X) 
%             disp([num,how]) 
end 
end  % End  of  Direction  Update 

%  Check  Termination 

if  max(abs(SD))<2*OPTIONS(2)  &  (GRAD'*(SD))  <  2*OPTIONS(3) 

ifOPT!ONS(l)>0 
disp(");disp("),disp("); 
disp(");disp("),disp("); 

disp('Optimization  Terminated  Successfully'), 
%  disp('Gradient  less  than  options(2)'). 

disp(['  NO  OF  ITERATIONS-',  num2slr(OPTIONS(10))]); 
end 

status=l; 
elseif  OPTIONS(10)>OPTIONS(14) 

ifOPT!ONS(l)>=0 
disp(");disp("),disp("); 
disp(");disp(");disp("); 

disp('Waming:  Maximum  number  of  iterations  has  been  exceeded'), 
dispC        -  increase  options(14)  for  more  iterations. ') 
end 

status=l; 
else 

%  Line  search  using  mixed  polynomial  interpolation  and  extrapolation. 
ifPCNT~=0 

while  PCNT  >  0 
OPTIONS!  10)=OFTIONS(10)+1; 
disp(");disp(");disp("); 
disp('Termination  Check  in  Progress'); 
disp(['Iterations:  ',num2str(OPTIONS(10))]), 
x(:)  =  XOUT; 

f  =  eval(evalstr), 
[PCNT,MATL,MATX,steplen,f,how]=searchq(PCNT,f,OLDX,... 

MATL,MATX,SD,GDOLD,OPTIONS(  1 8),  how); 
OPTIONS(18)=steplen; 
XOUT=OLDX+steplen*SD; 
if  abs(steplen)  <  le-6,  PCNT=0;  status=l ;  end 
end 

else 

x(:)=XOUT; 
OPTIONS(10)=OPTIONS(10)+1; 


disp("),disp(");disp("); 
disp('Tennination  Check  in  Progress'); 
disp(['Iterations:  \num2str(OFnONS(10))]); 
f  =  eval(evalstr); 
end 
end 
end 

x(:)=XOUT, 

disp("),disp(");disp("); 

disp(");disp(");disp("); 

disp(");disp(");disp("); 

disp('Final  Evaluation  in  Progress'); 

f  =  eval(evalstr); 

iff>FIRSTF 

OPTIONS(8)  =  FIRSTF; 

x(:)=OLDX; 

else 

OPTIONS(8)  =  f; 

end 


F.    MainMin 

%  Filename  is  "MainMin. m" 

%  This  is  the  routine  used  by  "MainOpt.m"  to  optimi/e  the  reference 
%  trajectory  polynomial  coefficients.  It  is  a  scaled  down  version 
%  of  the  dual  arm  spacecraft  program,  "Main2.m".    This  version  does 
%  not  integrate  the  state  variables  not  include  a  Lyapunov  controller. 
%  The  only  integration  that  does  take  place  is  the  optimization  cost 
%  function. 

function  [JOpt]  =  MainMin(UpCoef,ConstMat,Flags) 

%clg;clear; 

format  compact;format  short; 

k  =  length(UpCoef); 

A543  =  inv(ConstMat(:,k+l  :k+3))*([l;  0;  0]  -  ConstMat(:,l:k)*UpCoef); 

Coef  =  [UpCoef;  A543J;     %  Reference  trajectory  polynomial  coefficients 

%  Reference  Maneuver  Start  and  Stop  Times 

TO  =  0; 

TfR=  10, 

TfC=  10; 

MetaFlag  =Flags(l); 

ContFlag  =  Flags(2); 

PertFlag  =  Flags(3); 

AMatFlag  =  Flags(4); 

WheelFlag  =  Flags(5); 

EOMFlag    =Flags(6); 

PInvFlag  =Flags(7); 

KEFlag    =  Flags(8); 

OutFlag    =Flags(9); 

Trace     =Flags(10); 

SymFlag    =  Flags(  1 1 ); 

By  Pass     =Flags(12); 

TorqFlag  =Flags(13), 

Tol  =  le-6;     %  Integration  tolerance 
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R2D  =  180/pi;  %  Conversion  factor  from  radians  lo  degrees 

%  Lengths  (m) 

LO  =  0.75,  %  Origin  to  left  shoulder 

LI  =0.5;  %  Left  upper  ami 

1,2  =  0.5;  %  Left  forearm 

LP  =  0.5;  %  Pay  load 

R2  =  0.5;  %  Right  forearm 

R 1  =  0.5;  %  Right  upper  arm 

R0  =  sqrt(2*0.75A2);    %  Origin  to  right  shoulder 

Ls  =  [L0;  L 1 ;  L2;  LP;  R2;  R 1 ;  R0], 

%  Member  masses  (kg) 

M0  =  5, 

ML1  =  1; 

ML2=  1; 

MR1  =  1; 

MR2=  1; 

MP  =  1; 

Ms  =  [M0;  ML1;  ML2;  MR1,  MR2;  MP]; 

%  Center  of  mass  distances  (m) 

LcO  =0; 

LcLl  =0.25; 

LcL2=0.25; 

LcRl  =0.25; 

LcR2  =  0.25. 

LcP  =0.25; 

CMs  =  [LcO,  LcLl;  LcL2;  LcRl;  LcR2;  LcP]; 

%  MOI  about  center  of  mass:  Ic  =  (l/12)*(mass)*(length)A2 

10  =  M0, 

%I0  =  0; 

IL1  =(1/12)*ML1  *L1A2; 

IL2  =  (1/12)*ML2*L2A2; 

IR1  =(1/12)  *MR1  *R1A2; 

IR2  =  (1/12)*MR2*R2A2; 

IP  =(1/12)*  MP  *LPA2; 

Is=  [10;  IL1;  IL2;  IR1;  IR2;  IP]; 

%  Nominal  initial  and  desired  final  locations  of  pay  load 

%  Point  Q  is  at  wrist  of  left  arm 

%  Point  P  is  at  wrist  of  right  arm 

Qx0n  =  0.125;    QyOn  =  1.5; 

Px0n  =  0.625;    Py0n=1.5, 

Qxf  =0.125;      Qyf  =  1.0; 

Pxf  =0.125,       Pyf  =  1.5; 

%  Nominal  initial  and  desired  final  locations  of  centerbody 
ThOO  =  0; 
Th0f  =  0/R2D; 

%  Arms  mount  locations  wrt  centerbody  coordinate  frame  (rad) 
ThL0  =  pi/2; 
ThRO  =  pi/4; 
AngConst(l)  =  ThL0; 
AngConst(2)  =  ThRO; 

%  Symmetric  geometry  to  center  arms  and  test  kinetic  energy 
if  SymFlag 
Th'LO  =  3*pi/4; 
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AngConst(l)  =  ThLO; 

RO  =  LO;    %  Origin  to  right  shoulder 
Ls(7,l)  =  R0; 

QxOn  =  -0.25;    QyOn  =  1.2; 
PxOn  =  0.25;    PyOn  =  1.2; 
end 

BoundC(  1 , 1 )  =  QxOn;  BoundC(  1.2)  =  QyOn; 

BoundC(2,l)  =  PxOn;  BoundC(2,2)  =  PyOn; 

BoundC(3,l)  =  Qxf;  BoundC(3,2)  =  Qvf. 

BoundC(4,l )  =  Pxf;  BoundC(4,2)  =  Pyf; 

BoundC(5, 1 )  =  ThLO;  BoundC(5,2)  =  ThRO; 

BoundC(6,l)  =  TO;  BoundC(6,2)  =  TfR; 

BoundC(7,l )  =  TfC;  BoundC(7,2)  =  ContFlag; 

BoundC(8, 1 )  =  AMatFlag;  BoundC(8,2)  =  WheelFlag; 

BoundC(9,l )  =  ThOO;  BoundC(9,2)  =  ThOf; 

BoundC(10,l)=  EOMFlag;  BoundC(10,2)=  PInvFlag; 

BoundC(  11,1)=  KEFlag;  BoundC(  1 1 ,2)=  By  Pass; 
BoundC(12,l)=TorqFlag; 

%  Weighting  Matrices 

%  Control  torques  are  calculated  to  minimize  the  following  cost  function: 

%    J  =  0.5*(u'*Wu*u  +  (A'*Lam)'*Wc*(A'*Lam )) 

if  WheelFlag 

Wu  =  eye(7);        %  Control  Torque  Weighting 
else 

Wu  =  eye(6); 
end 

%if  WheelFlag 
%  Wu  =  zeros(7,7); 
%else 

%  Wu  =  zeros(6,6), 
%end 

%Wu(4,4)=le5; 
%Wu(7,7)=le5; 
%Wu(2,2)=lelO; 
%Wu(5,5)=lelO; 

Wc  =  zeros(8,8),     %  Constraint  Force  Weighting 
%Wc  =  eye(8); 

%%%%%%%%%%%%%%%%% 
%%  INITIAL  CONDITIONS  %% 
%%%%%%%%%%%%%%%%% 
%  Desired  Initial  Pavload  Parameters 
ThPO  =  atan2(Py0n-Qy0n,Px0n-Qx0n); 
XcO  =  0.5  *  (PxOn  +  QxOn); 
YcO  =  0.5  *(PyOn  + QyOn); 

QxO  =  QxOn; 
QyO  =  QyOn; 
PxO  =  PxOn; 
PyO  =  PyOn; 

%  Initial  State 
X0  =  0; 

%%%%%%%%%%%%% 

%%  INTEGRATION  %% 

%%%%%%%%%%%%% 

%  RefMin2  uses  change  in  angular  momentum  to  find  wheel  command  torque 

fTJInt,J]  =  odemin(,RefMin2,)T0,TfR,X0,Tol,Trace.Ls,Ms.CMs,Is.BoundC,... 
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Wu,Wc,Coef,ConslMat); 
%  Optimization  cost  function  is  integral  of  J 
k  =  lenglh(T); 
JOpt  =  Jlnt(k); 
%JOpt  =  max(abs(J)); 


G.  MainOpt 

%  Filename  is  "MainOpt. m" 

%  This  routine  optimizes  the  dual  arm  spacecraft  cost  function 
%  by  changing  the  polynomial  coefficients  which  describe  the 
%  reference  trajectory.  It  calls  "Main2.m" 

%clear 

clc 

home 

format  compact 

format  short 

I  JpCoefO  =  [0];  %  Starting  Guess 

%UpCoefO  =  UpCoef;  %  Use  last  values  for  starting  guess 

k  =  length(UpCoefO); 

options      =  [];  %  Default  values 

options(l)  =  0;  %  Display  dunng  optimization  cycle:   l=On,  0=Off 

options(14)  =  100*k;        %  Maximum  number  of  iterations 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  Flags  during  optimization  %% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

MetaFlag  =  0,  %  Creates  metafile  named  "main. met" 

ContFlag  =  0;    %  Controller  Status  Flag:  l=On;  0=Off 

PertFlag  =  0;      %  Perturbation  Flag  (0=no  perturbation,  1  =perturbation) 

%  The  perturbation  is  to  check  the  controller  by 

%  disturbing  the  actual  initial  state  away  from  nominal. 

%  The  reference  torques  are  based  on  nominal. 
AMatFlag  =  0;    %  Size  of  A  matrix:  0  =  4x8  (Free  Centerbody ) 

%  1  =  5x8  (Fixed  Centerbodv)  ' 

WheelFlag  =  1;  %  Centerbody  Reaction  Wheel  (l=On,  0=Off) 
EOMFlag  =  8;    %  Specifies  number  of  cost  function  constraint  equations 

%  3  =  only  payload  equations 

%  5  =  only  spacecraft  equations 

%    any  other  value  =  all  8  equations 
PInvFlag  =  1 ;     %  Psuedo-Inverse  Flag  (for  use  in  finding  reference  torques) 

%      1  =  Use  psuedo-inverse 

%      0  =  Use  inverse 
KEFlag  =  0;      %  KE  Test  Flag 

%      1  =  Nonzero  velocity  initial  conditions 

%      0  =  Zero  velocity  initial  conditions 
OutFlag  =  0;     %  Output  Flag 

%      1  =  Display  output 

%      0  =  Don't  display  output 
Trace  =1;  %  Observe  integration 

%      1  =  Observe 

%      0  =  Don't  observe 
OptFlag  =  0,     %  Optimization  Flag 

%      1  =  Perform  optimization 

%      0  =  Don't  perform  optimization 
SymFlag  =  0;     %  Symmetric  Geometry  Flag 

%      1  =  Symmetric  geometry 
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%      0  =  Nonsymmetric  geometry 
ByPass  =  0;       %  Torque  calculation  bypass  flag 

%      1  =  Bypass 

%     0  =  Use  inverse  kinematics 
TorqFlag=  0;     %  Torques  to  use  if  bypass  enabled 

%      0  =  No  Torques  (Drift) 

%      1  =  One  Shoulder  Torque 

%      2  =  Symmetric  Shoulder  Torques 
TorqCap  =  0;     %  Maximum  limit  on  wheel  torque 

%      1  =  Enabled 

%      0  =  Disabled 
TorqMax  =  0.075;%  Limit  on  wheel  torque  if  TorqCap  enabled 
Flagsl(l)  =  MetaFlag; 
Flags  1(2)  =  ContFlag; 
Flagsl(3)  =  PertFlag; 
Flags]  (4)  =  AMatFlag; 
Flagsl(5)  =  WheelFIag; 
Flagsl(6)  =  EOMFlag; 
Flagsl(7)  =  PInvFlag; 
Flagsl(8)  =  KEFlag; 
Flagsl(9)  =  OutFIag; 
Flagsl  (10)=  Trace; 
Flags]  (11)=  SvmFlag; 
Flagsl  (1 2)=  ByPass; 
Flagsl (13)=  TorqFlag: 
Flagsl(14)=  TorqCap; 
Flagsl  (15)=  TorqMax, 
%%%%%%%%%%%%%%%% 
%%  Flags  after  optimization  %% 
%%%%%%%%%%%%%%%% 
Flags2  =  Flagsl 


Flags2(l)  =  0 
Flags2(2) =  1 
Flaes2(3)  =  0 
Flags2(5) =  1 
Flags2(8) =  0 
Flags2(9)=  1 
Flags2(10)=  1 
Flags2(ll)=0 
Flags2(12)=0 
Flags2(13)=0 


%  MetaFlag:      l=On,  0=Off  (File  is  "main. met") 
%  Controller  Flag:       1  =On,  0=Off 
%  Perturbation  Flag:     1  =On,  ()=0ff 
%  Wheel  Flag:   l=On,  0=Off 
%  Kinetic  Energy  Flag:   l=On,  0=Off 
%  Output  Flag:   l=On,0=Ofl' 
%  Trace  Flag:    1  =On,  0=Off 
%  Symmetric  Geometry  Flag:   l=Sym,  0=NonSym 
%  Inverse  Kinematics  Bypass:    l=Bypass,  0=lnverse  Kinematics 
%  Torq  Flag:  0=No  Torq,  l=One  Torq.  2=Two  Symmetric  Torqs 
%     Torq  Flag  is  for  when  the  bypass  is  enabled 
Flags2(14)=0;         %  TorqCap:   l=On,  0=Off 
Flags2(15)=  0.075;%  Limit  on  maximum  wheel  torque 
Flags2(16)=0;         %  DocFlag:   l=On,0=Off 

%    separate  meta  files  for  each  page  ("doc#.met") 
DiaFlag    =0;  %  Diary  Flag 

%      1  =  Create  diary  file  "main  dia" 
%      0  =  No  diar>'  file 
ConstMat  =  ones(3,k+3); 
forn=l:k+3 
ConstMal(2,n)  =  k+6-n; 

ConstMat(3,n)  =  ConstMat(2,n)*(ConstMal(2,n)-l), 
end 
if  OpfFlag 

[UpCoeLoptions]  =  fminu2('MainMin,,UpCocf0,options,[],ConstMat,Flagsl ); 
end 

if  DiaFlag 
diary  main  dia 
end 


130 


if—OptFlag 
UpCoef=UpCoefO; 
end 
[Jlnt]  =  Main2(UpCoef,ConstMat,Flags2); 

%  Plot  position,  velocity,  &  acceleration  reference  trajectories 

k  =  lengthfUpCoef); 

A543  =  inv(ConstMat(:,k+l:k+3))*([l;  0;  0]  -  ConstMat(:,l:k)*UpCoef); 

Coef  =  [UpCoef,  A543J,     %  Reference  trajectory  polynomial  coefficients 

k  =  length(Coef), 

Steps  =  21; 

form  =  1:  Steps 

Tau  =  (m-l)/(Steps-l); 

for  n=l:k 

CTau(k+l-n)  =  Coef(k+l-n)*TauA(n+2); 
CTaud(k+l-n)  =  Coef(k+l-n)*TauA(n+l); 
CTaudd(k+l  -n)  =  Coef(k+l  -n)*TauA(n); 

end 

W(m)    =ConstMat(l,:)*CTau'; 

Wd(m)  =  ConstMat(2,:)*CTaud'; 

Wdd(m)  =  ConstMat(3,:)*CTaudd'; 
end 
clg 

T=0: 1 /(Steps- 1):1; 
subplot(221) 

plot(T,W);title('Position  vs  Normalized  Time'), 
xlabel('Tau  (sec)');ylabel('Position'); 
subplot(222) 

title('Reference  Trajectories') 
subplot(223) 

plot(T,Wd);tille('Velocity  vs  Normalized  Time'); 
xlabel(Tau  (sec)');ylabei('Velocity'); 
subplot(224) 

plot(T,Wdd);title(' Acceleration  vs  Noimalized  Time'); 
xlabel('Tau  (sec)');ylabel('Acceleration'); 
if  Flags2(l) 

meta  main 
end 
if  Flags2(16) 

meta  doc6 
end 
pause 

disp('Initial  guess  for  highest  order  coefficienls');disp(l  IpCoefO'); 
disp('Coefficients  in  descending  order');disp(Coef); 
disp('Integral  of  Cost  Function, (JIntAbs  &  .UntSqr)'),disp(.IInt), 
ifOptFlag 

disp('lterations');disp(options(10)); 
end 
diaiy  off 


H.  Main2 

%  Filename  is  "Main2.m" 

%  This  routine  is  the  driver  for  the  dual  arm  spacecraft  problem 

%  but  is  called  by  "MainOpt.m"  after  the  polynomial  reference  trajectory 

%  coefficients  have  been  optimized. 
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function  f JIntTotal]  =  Main2(UpCoef,ConstMat,Flags) 

%  Calculate  the  coefficients  for  orders  five,  four,  and  three. 

%  Include  these  with  the  higher  order  coefficients  in  a  vector. 

k  =  lcngth(UpCoef); 

A543  =  mv(ConstMat(,k+l  :k+3))*([l;  0;  0]  -  ConsuMat(:,l:k)*UpCoef); 

Coef  =  fUpCoef ;  A543],     %  Reference  trajectory  polynomial  coefficients 

%%%%%%%% 

%%  Times  %% 

%%%%%%%% 

%  Reference  Maneuver  Start  and  Stop  Times  and  Controller  Stop  Times 

%  Setting  the  controller  time  longer  than  the  reference  maneuver  time 

%  ensures  that  the  controller  eliminates  any  errors  remaining  after  the 

%  reference  trajectory  should  be  complete.  To  exercise  the  controller 

%  only  with  no  reference  trajectory,  set  the  reference  maneuver  stop 

%  time  to  a  negative  value. 

TO  =  0; 

TfR=  10; 

TfC=  10; 

MetaFlag  =  Flags(l); 

ConlFlag  =  Flags(2); 

PertFlag  =  Flags(3); 

AMatFlag  =  Flags(4); 

WheelFlag  =  Flags(5); 

EOMFlag    =Flags(6); 

PInvFlag  =Flags(7); 

KEFlag     =  Flags(8); 

OutFlag   =  Flags(9); 

Trace     =Flags(10); 

SvmFlag    =Flags(ll); 

By  Pass     =Flags(12); 

TorqFlag  =Flags(13); 

TorqCap    =Flags(14); 

TorqMax    =  Flags(  1 5); 

DocFlag    =Flags(16); 

Pert  =  -10;     %  Perturbation  of  initial  payload  angle,  ThetaP  (deg) 
Tol  =  1  e-6;     %  Integration  tolerance 

Interval  =  3;  %  Stick  figure  drawing  includes  every  Interval'th  time 
R2D  =  180/pi;  %  Conversion  factor  from  radians  to  degrees 

%%%%%%%%%%%%%% 

%%  System  Parameters  %% 

%%%%%%%%%%%%%% 

%  Lengths  (m) 

L0  =  0.75;  %  Origin  to  left  shoulder 

LI  =0.5;  %  Left  upper  arm 

L2  =  0.5;  %  Left  forearm 

LP  =  0.5;  %  Pavload 

R2  =  0.5;  %  Right  forearm 

R 1  =  0.5;  %  Right  upper  arm 

R0  =  sqrt(2*0.75A2);    %  Origin  to  right  shoulder 

Ls  =  [LO;Ll;L2;LP;R2;Rl;RO]; 

%  Member  masses  (kg) 
M0  =  5; 
ML1  =  1; 
ML2  =  1; 
MR1  =  1; 
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MR2=  1; 

MP  =  1 ; 

Ms  =  [MO.  ML1;  ML2;  MR1;  MR2;  MP]; 

%  Center  of  mass  distances  (m) 

I.cO  =0; 

LcLl  =0.25; 

Lcl,2  =  0.25; 

LcRl  =0.25; 

LcR2  =  0.25; 

LcP  =0.25; 

CMs=  [LcO;  LcLl;  LcL2;  LcRl;  LcR2;  LcP], 

%  MOI  about  individual  centers  of  mass 

%  Arms  are  modelled  as  slender  rods:  Ic  =  ( l/12)*(mass)*(length)A2 

10  =M0, 

IL1  =(1/12)*ML1  *L1A2; 

IL2  =  (1/12)*ML2*L2A2; 

IR1  =  (1/12)*MR1  *R1A2; 

IR2  =  (1/12)*MR2*R2A2; 

IP  =(1/12)*  MP  *LPA2, 

Is=[10;ILl;IL2;IRl;IR2;IP]; 

%  Nominal  initial  and  desired  final  locations  of  pay  load 

%  Point  Q  is  at  wrist  of  left  arm 

%  Point  P  is  at  wrist  of  right  arm 

Qx0n  =  0.125;    QyOn  =  1.5; 

Px0n  =  0.625;    PvOn  =  1.5; 

Qxf  =0.125;    Qvf  =  1.0; 

Pxf  =0.125;    Pyf  =  1.5; 

%  Nominal  initial  and  desired  final  locations  of  centerbody 

ThOO  =  0/R2D; 

Th0f=0/R2D; 

%  Arms  mount  locations  wrt  centerbody  coordinate  frame  (rad) 
ThLO  =  pi/2; 
ThRO  =  pi/4; 
AngConst(l)  =  ThL0; 
AngConst(2)  =  ThRO; 

%  Symmetric  geometry  to  center  aims  and  test  kinetic  energy 

if  SymFlag 

ThLO  =  3*pi/4; 

AngConst(l)  =  ThL0; 

R0  =  L0;    %  Origin  to  right  shoulder 

Ls(7,l)  =  R0; 

Qx0n  =  -0.25;   QyOn  =  1.2; 

Px0n=  0.25;    PyOn  =  1.2; 

end 

%  Assemble  information  required  in  other  subroutines  into  a  matrix 

BoundC(l,l)  =  QxOn;  BoundC(l,2)  =  QvOn; 

BoundC(2,l)  =  PxOn;  BoundC(2,2)  =  PyOn; 

BoundC(3,l)  =  Qxf,  BoundC(3,2)  =  Qvf; 

BoundC(4,l )  =  Pxf;  BoundC(4,2)  =  Pyf; 

BoundC(5,l )  =  ThLO;  BoundC(5,2)  =  ThRO. 

BoundC(6, 1 )  =  TO;  BoundC(6,2)  =  TfR; 

BoundC(7,l)  =  TfC;  BoundC(7,2)  =  ContFlag; 

BoundC(8,l)  =  AMatFlag;  BoundC(8,2)  =  WheelFlag; 

BoundC(9, 1 )  =  ThOO;  BoundC(9,2)  =  ThOf ; 
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BoundC(10,l)=  EOMFlag;  BoundC(10,2)=  PInvFlag; 

BoundC(ll,l)=  KEFlag,  BoundC(ll,2)=  ByPass; 

BoundC(12,l)=TorqFlag; 

BoundC(  1 3, 1  )=  TorqCap;  BoundC(  1 3,2)=  TorqMax; 

%    Gip  are  gains  for  angle  i  position  error 
%    Giv  are  gains  for  angle  i  velocity  error 
Gpos  =  0.5;      %  Position  error  gain 
Gvel  =  0.2;      %  Velocity  error  gain 
Gains  =  [Gpos;  Gvel], 

%  Weighting  Matrices 

%  Control  torques  are  calculated  to  minimize  the  following  cost  function: 

%    J  =  0.5*(u'*Wu*u  +  (A,*Lam)'*Wc*(A'*Lam)) 

%    Wu  is  the  control  torque  weighting  matrix 

%    Wc  is  the  constraint  force  weighting  matrix 

ifWheelFlag 

Wu  =  eye(7);         %  Control  Torque  Weighting 
else 

Wu  =  eye(6); 
end 

%if  WheelFlag 
%  Wu  =  zeros(7,7); 
%else 

%  Wu  =  zeros(6,6); 
%end 

%Wu(4,4)=le5;        %  Penalty  on  wrist  motors  for  free  centerbodv  case 
%Wu(7,7)=le5; 

%Wu(2,2)=lel0;        %  Penalty  on  wrist  motors  for  fixed  centerbody  case 
%Wu(5,5)=lel0; 

Wc  =  zeros(8,8);     %  Constraint  Force  Weighting 
%Wc  =  eye(8); 

%%%%%%%%%%%%%%%%% 
%%  INITIAL  CONDITIONS  %% 
%%%%%%%%%%%%%%%%% 
%  Desired  Initial  Payload  Parameters 
ThPO  =  atan2(Py0n-Qy0n,Px0n-Qx0n); 
XcO  =  0.5  *  (PxOn  +  QxOn); 
Yc0  =  0.5*(Py0n  +  Qy0n); 

ifPertFlag  %  Perturbation  enabled 

ThPO  =  ThPO  +  Pert/R2D;  %  Perturb  payload  angle 

0x0  =  XcO  -  LcP*cos(ThP0);        %  Perturb  arm  end  points 

QvO  =  YcO  -  LcP*sin(ThP0); 

PxO  =  XcO  +  (LP-LcP)*eos(ThP0); 

PyO  =  YcO  +  (LP-LcP)*sin(ThP0); 
else  %  No  Perturbation 

QxO  =  QxOn; 

QyO  =  QyOn; 

PxO  =  PxOn; 

PyO  =  PyOn; 
end 
PertCrd  =  [QxO  QyO  PxO  PyO]; 

%  Left  Arm 

%  Elbow  is  left  of  line  from  arm  base  to  Q  (RQ) 

LSx  =  L0  *  cos(Th00  +  ThLO); 

LSy  =  LO  *  sin(ThOO  +  ThLO); 

RQ  =  sqrt((QxO-LSx)A2+(QyO-LSy)A2);        %  Length  from  arm  base  to  Q 

Betal  =  atan2(QyO-LSy,QxO-LSx);  %  Angle  from  arm  base  to  RQ 
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%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2bc) 

%  Applv  to  find  angle  between  RQ  and  Link  LI 

Num  =  LlA2  +  RQA2-L2A2; 

Den  =  2  *  L 1  *  RQ, 

Beta2  =  acos(Num/Den);  %  Angle  bom  RQ  to  Link  1 

ThI  .10  =  (Beta  1  +  Beta2)  -  (ThOO  +  ThLO);    %  Theta  L 1 

%  Use  law  of  cosines  to  find  the  interior  angle  at  the  elbow 

Num  =  LlA2  +  L2A2-RQA2; 

Den  =  2  *  L 1  *  L2; 

Beta3  =  acos(NumZDen); 

ThL20  =  -(pi-Beta3), 

%  Right  Arm 

%  Elbow  is  right  of  line  from  arm  base  (shoulder)  to  P  (wrist)  (RP) 

RSx  =  R0  *  cos(Th00  +  ThRO); 

RSy  =  R0  *  sin(ThOC)  +  ThRO); 

RP  =  sqrt((PxO-RSx)A2+(PyO-RSy)A2);  %  Length  from  arm  base  to  P 

Bctal  =  atan2(Py0-RSy,Px0-RSx);         %  Angle  from  arm  base  to  RP 

%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2bc) 

%  Applv  to  find  angle  between  RP  and  Link  Rl 

Num='RlA2  +  RPA2-R2A2; 

Den  =  2*Rl  *  RP; 

Beta2  =  acos(Num/Den);  %  Angle  from  Link  R 1  to  RP 

Beta4  =  Betal  -  (ThOO  +  ThRO); 

ThR10  =  -(Beta2-Beta4); 

Num  =  RlA2  +  R2A2-RPA2; 

Den  =  2*Rl  *  R2; 

Beta.l  =  acos(Num/Den), 

ThR20  =  pi  -  Beta3; 

%  Desired  Initial  State 

X0  =  [ThOO,  ThLlO;  ThL20;  ThRlO;  ThR20;  ThPO,  XcO;  YcO;... 
0;     0;      0;      0;      0;      0,    0;    0]; 

%%%%%%%%%%%%%%%%%%%% 

%%  Kinetic  Energy  Test  Conditions  %% 

%%%%%%%%%%%%%%%%%%%% 

%  Specify  Payload  and  Centerbody  Initial  Rates 

%  Compatible  Rates  for  the  Redundant  Coordinates  are  calculated 

ifKEFlag 

ThPdO  =  0/R2D;    %  Rates  to  specify 

Xcd0  =  -0.03; 

Ycd0  =  -0.03; 

ThOdO  =  0/R2D; 

%%%%%%%%%%% 

%%  LEFT  ARM  %% 

%%%%%%%%%%% 

%  IQxd;  Qyd]  =  [Hl]*Th0d  +  [H2]*Thd 

%  Qxd  &  Qyd  are  x  &  y  components  of  point  Q  inertial  velocity. 

%  Thd  =  [ThL  ldot;  ThL2dot] 

%  H  matrices  are  made  from  expressing  the  x  &  y  components  of  Q  in 

%  terms  of  L0,  ThO,  ThLO,  L 1 ,  ThL  1 ,  L2,  and  ThL2. 
%  Qx=L0*cos(Th0+ThL0)+L  1  *cos(Th()+ThL()+TL  1  )+L2*cos(Th()+. 

%  ThL0+ThLl+ThL2) 

%  Qy=L0*sin(Th0+ThL0)+Ll*sin(Th0+ThL0+ThLl)+L2*sin(Th0+. 

%  ThL0+ThLl+ThL2) 

%  The  differentiation  of  these  equations  lead  to 

%  [Qxd;  Qyd]  =  [Hl]*Th0d  +  [H2]*Thd  which  can  be  solved  for  Thd 

QxdO  =  XcdO  +  LcP  *  ThPdO  *  sin(ThPO); 

QydO  =  YcdO  -  LcP  *  ThPdO  *  cos(ThPO); 

II'2(1,2)  =  -L2*sin(Th00+ThL0+ThL10+ThL20); 
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H2(  1,1)=  H2(  1 ,2)  -  L 1  *sin(ThOO+ThLO+ThI .  1 0); 

112(2,2)=  L2*cos(ThOO+ThLO+ThL10+ThL20); 

H2(2,l )  =  H2(2,2)  +  Ll*cos(Th()0+ThLO+ThL  10); 

111(1,1)=  H2(  1,1)-  L0*sin(Th00+ThL0); 

H 1  (2, 1 )  =  H2(2, 1 )  +  L0*cos(Th00+ThL0); 

ThdO  =  inv(H2)  *  ([QxdO;  QydO]  -  HI  *ThOdO); 

%  Angle  rates 

ThLldO  =  ThdO(l); 

ThL2dO  =  ThdO(2); 

%%%%%%%%%%%% 

%%  RIGHT  ARM  %% 

%%%%%%%%%%%% 

%  The  development  is  similar  to  the  left  arm 
%  Px=RO*cos(ThO+ThRO)+R  1  *cos(ThO+ThRO+ThR  1  )+R2*cos(Th()+. 

%  ThR0+ThRl+ThR2) 

%  Py=RO*sin(ThO+ThRO)+Rl*sin(Th()+ThRO+ThRl)+R2*sin(Th()+... 

%  ThR0+ThRl+ThR2) 

%  [Pxd;  Pyd]  =  [Hl]*ThOd  +  [H2]*Thd 

PxdO  =  XcdO  -  (LP  -  LcP)  *  ThPdO  *  sin(ThPO); 

PydO  =  YcdO  +  (LP  -  LcP)  *  ThPdO  *  cos(ThPO); 

112(1,2)  =  -R2*sin(Th()0+ThR0+ThR10+ThR2O); 
112(1,1)=  H2(  1 ,2)  -  R 1  *sin(ThOO+ThRO+ThR  10); 
H2(2,2)  =  R2*cos(ThOO+ThRO+ThR10+ThR20); 

H2(2, 1 )  =  H2(2,2)  +  R 1  *cos(ThOO+ThRO+ThR  1 0); 
H 1  ( 1 , 1 )  =  H2(  1 , 1 )  -  R0*sin(Th00+ThR0); 

H 1  (2, 1 )  =  H2(2, 1 )  +  R0*cos(Th00+ThR0); 

ThdO  =  inv(H2)  *  ([PxdO;  PydO]  -  Hl*ThOdO); 

%  Angle  rates 

ThRldO  =  ThdO(l); 

ThR2dO  =  ThdO(2); 

XO  =  [ThOO;  ThLlO;     ThL20;    ThRlO;  ThR20;    ThPO;    XcO.    YcO;... 
ThOdO;  ThLldO;  ThL2dO;  ThRldO;  ThR2dO;  ThPdO;  XcdO,  YcdO], 
end 

%%%%%%%%%%%%%%%% 
%%  FINAL  CONDITIONS  %% 
%%%%%%%%%%%%%%%% 
%  Desired  Final  Pay  load  Angle 
ThPf  =  atan2(Pyf-Qyf,Pxf-Qxf); 

%  Left  Arm 

%  Elbow  is  left  of  line  from  arm  base  to  Q  (RQ) 

LSx  =  LO  *  cosfThOf  +  ThLO), 

LSv  =  LO  *  sin(ThOf  +  ThLO); 

RQ  =  sqrt((Qxf-LSx)A2+(Qyf-LSy)A2);        %  Length  from  arm  base  to  Q 

Betal  =  atan2(Qyf-LSv,Qxf-LSx);  %  Angle  from  arm  base  to  RQ 

%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2bc) 

%  Apply  to  find  angle  between  RQ  and  Link  L 1 

Num  =  LlA2  +  RQA2-L2A2; 

Den  =  2  *  L 1  *  RQ; 

Beta2  =  acos(Num/Den);  %  Angle  from  RQ  to  Link  1 

ThI .  1  f  =  (Beta  1  +  Beta2)  -  (ThOf  +  ThLO);    %  Theta  L 1 

%  Use  law  of  cosines  to  find  the  interior  angle  at  the  elbow 

Num  =  LlA2  +  L2A2-RQA2; 

Den  =  2*Ll  *L2; 

Beta3  =  acos(Num/Den), 

ThL2f=-(pi-Beta3); 

%  Right  Arm 

%  Elbow  is  right  of  line  from  arm  base  (shoulder)  to  P  (wrist)  (RP) 

RSx  =  RO  *  cos(ThOf  +  ThRO). 
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KSv  =  RO  *  sin(ThOf  +  ThRO); 

RP  =  sqrt((Pxf-RSx)A2+(Pyf-RSy)A2);  %  Length  from  arm  base  to  P 

Beta  1  =  atan2(Py  f-RSy,Pxf-RSx);         %  Angle  from  arm  base  to  RP 

%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2be) 

%  Apply  to  find  angle  between  RP  and  Link  Rl 

Num  =  RlA2  +  RPA2-R2A2; 

Den  =  2*Rl  *  RP; 

Beta2  =  acos(Num/Den);  %  Angle  from  Link  R 1  to  RP 

Beta4  =  Beta  1  -  (ThOf  +  ThRO); 

ThRlf=-(Beta2-Beta4); 

Num  =  R 1 A2  +  R2A2  -  RPA2; 

Den  =  2*Rl  *  R2; 

Beta3  =  acos(Num/Den); 

ThR2f=pi-Beta3; 

%  Desired  Final  State 
Xcf=0.5*(Pxf +  Qxf); 
Ycf=0.5*(Pyf  +  Qyf); 

QfDes  =  [ThOf;  ThLlf;  ThL2f;  ThRlf;  ThR2f;  ThPL  Xcf;  Ycf;... 
0;         0;         0;  0,  0;  0;       0;      ()]; 

ifOutFlag 

%%%%%%%%%%%%%%%%% 

%%  PROBLEM  SUMMARY  %% 

%%%%%%%%%%%%%%%%% 

disp('PROBLEM  SUMMARY') 

disp(") 

disp('Initial  Angles  (deg)') 

dispCInitial  Angular  Rates  (deg/sec)') 

disp('Desired  Final  Angles  (deg)') 

disp('     ThetaO   ThetaLl    ThetaL2    ThetaRl    ThctaR2     ThetaP') 

disp(X0(l:6)'*R2D) 

disp(X0(9:14)'*R2D) 

disp(QfDes(l:6)'*R2D) 

disp(") 

disp('Payload  Coordinates  (m)') 

dispC  Nominal  Initial,  Perturbed  Initial,  and  Final') 

disp('       Qx         Qy         Px         Py') 

TableCrd  -  [QxOn  QyOn  PxOn  PyOn;  PertCrd;  Qxf  Qyf  Pxf  Pyf], 

disp(TableCrd) 

disp(") 

disp('Arm  Mounting  Locations  wrt  Centerbody  Coordinate  Frame  (deg)') 

disp(ThL0*R2D);disp(ThR0*R2D) 

disp(") 

disp('Start,  Reference  Manuever  Slop,  &  Simulation  Stop  Times  (sec)') 

disp(T0);disp(TfR);disp(TfC) 

disp(") 

disp('Controller  Status  (1  =  On;  0  =  Off)') 

disp(ContFlag) 

disp(") 

disp('Perturbation  Status  (1  =  On;  0  =  Off)') 

disp(PertFlag) 

disp(") 

disp('Centerbody  Status  in  Forward  EOM  (1  =  Fixed,  0  =  Free)') 

disp(AMatFlag) 

disp(") 

disp('Reaction  Wheel  Status  (1  =  On;  0  =  Off)') 

disp(WheelFlag) 

disp(") 

disp('Number  of  Equations  in  Cost  Function  Constraint  (3,  5  or  8)') 

disp(EOMFlag) 
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disp(") 

disp('Psuedo-Inverse  Status  (1  =  On;  0  =  Off)') 

disp(PInvFlag) 

disp(") 

disp('Nonzero  Initial  Velocity  Status  (1  =  On.  0  =  Off)') 

disp(KEFlag) 

dispf) 

disp('Geometry  Status  (1  =  Symmetric,  0  =  Nonsymmetric)') 

disp(SymFlag) 

disp(") 

disp('Inverse  Kinematics  Bypass  Status  (1  =  Bypass.  0  =  Use  inv.  kinematics)') 

disp(ByPass) 

disp(") 

disp('Torques  if  Bypass  Enabled  (0=None,  l=One,  2=T\vo  Symmetnc)') 

disp(TorqFlag) 

disp(") 

disp('Reaction  Wheel  Torque  Cap  Status  (l=Enabled,  0=Disabled)') 

disp(TorqCap) 

if  TorqCap 

disp('Limit  on  Wheel  Torque'); 

disp(TorqMax); 
end 
disp(") 

disp( 'Controller  Gains  (position  and  velocity)') 
disp('       Gpos       Gvel') 
disp(Gains') 
disp(") 
dispCCost  Function:  J  =  0.5*(uT*Wu*u  +  (AT*Lam)T*Wc*(AT*Lam))') 

disp('      where T  signifies  transpose') 

disp('Control  Torques  Weighting  Matrix,  Wu') 

disp(Wu) 

%disp('Constraint  Forces  Weighting  Matrix,  We') 

%disp(Wc) 

end     %  End  of  OutFlag  branch 

%%%%%%%%%%%%% 

%%  INTEGRATION  %% 

%%%%%%%%%%%%% 

%  "ode"  is  a  variable  step  size  Runge-Kutta  integrator  function 

%  supplied  with  MATLAB.  "ode2"  is  the  same  as  "ode"  in  its  function 

%  but  permits  the  passing  of  more  variables  into  and  out  of  the  function. 

[T,X,Torq,TorqRef,Aqdot,J,Res,LHS,RIIS,Delq]  =  ... 

ode2('Eqn2',TO,TfC,XO,Toi;rrace,Ls.Ms,CMs.Is.BoundC,... 

Gains,QfDes,Wu,Wc,Coef,ConstMat); 
k  =  length(T), 
JInt  =  X(:,17:18); 
JIntTotal  =  X(k,17:18); 

if  OutFlag 

%%%%%%%%%% 
%%  OUTPUT  %% 
%%%%%%%%%% 

clg; 

%  Angle  Histories 
n  =  length(T); 
Q  =  X(:,1:6); 
subplot(221) 

plot(T,Q(:,l)*R2D,T,Q(:,2)*R2D,T,Q(:,3)*R2D,... 
T,Q(:,4)*R2D,T,Q(:,5)*R2D,T,Q(:,6)*R2D); 


138 


hold  on 
plot(T(n),QfDes(l)*R2D,,*,,T(n),QfDes(2)*R2D,,*,,T(n),QfDes(3)*R2D;*',... 

T(n),Qn)es(4)*R2D),*,>T(n),QfDes(5)*R2D,,*M(n),Qn^cs(6)*R2D:*'), 
title('Thetas  vs  Time'); 
xlabel('Time  (sec)');ylabel('Angles  (dcg)'); 
hold  off 

%  Angle  Rate  Histories 
Qdot  =  X(:,9:14); 
subplot  (223) 
plot(T,Qdot(:,l)*R2D,T,Qdol(:)2)*R2D,T,Qdot(:,3)*R2D,... 

T,Qdot(:,4)*R2D,T,Qdot(:,5)*R2D,T,Qdot(:,6)*R2D); 
titlefThetaOots  vs  Time'); 
\label('Time  (sec)');ylabel('Angle  Rates  (deg/sec)'); 

%Departures  from  Referenee  Trajectory 
if  ~BvPass 

subplot(222) 

plot(TDelq(l,:)*R2D,T,Delq(2,:)*R2D,TDclq(3.:)*R2D,... 
T,Delq(4,:)*R2D,T,Delq(5,:)*R2D,T,Delq(6,:)*R2D); 

title('Displacement  Errors  vs  Time'); 

xlabeK'Time  (sec)');ylabel('Q-QRef  (deg)'); 
end 

if  MetaFlag 

meta  main 
end 
if  DoeFlag 

meta  doc  1 
end 
pause 

%  Draw  Motion 

Angles  =  Q(:, 1:6); 

[Xcoord,Yeoord]  =  Draw3(Ls,AngConst,Angles,Interval); 

if  MetaFlag 

meta  main 
end 
if  DoeFlag 

meta  doc2 
end 
pause 

disp(-); 

disp('STATE  VECTOR  TIME  HISTORY:'); 

disp('Angles  (deg)') 

Tablel  =  [T  X(:,1:6)*R2D], 

disp('      Time      ThetaO    ThetaLl    ThetaL2    ThetaRl    ThetaR2     ThetaP'); 

disp(Tablel) 

pause 

disp("); 

disp('Angle  Rates  (deg/sec)') 

Table2  =  [T  Qdot(:,l  :6)*R2D]; 

disp('      Time      ThOdot    ThLldot    ThL2dot    ThRldot    ThR2dot     ThPdol'); 

disp(Table2) 

pause 

if  ~ByPass 
disp("); 

disp('TRA.IECTORY  ERROR  TIME  HISTORY'): 
disp('Angles  (deg)') 
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Table2a  =  |T  R2D*Delq(l:6,:)']; 

disp('      Time      DelThO    DelThLl    DelThL2    DclThRl    DelThR2     DclThP); 

disp(Table2a) 
end 
pause 

%  Reference  Torque  Histories 

clg. 

if  TfR  >  0 

if  TfR  <  TfC 

|r,s]  =  size(TorqRef ), 
TorqRef  =  [TorqRef  zeros(s,  1 )] ; 
TRef=[T(l:r);TfR], 

else 

TRef=T; 

end 

subplol(22I) 

plot(TRef,TorqReO; 

title('Reference  Torques  vs  Time'), 

xlabel('Time  (sec)');ylabel('Reference  Torques'); 
end 

%  Command  Torque  Histories 
%Torq  =  [Torq,  zeros(4,l)]; 
k=n; 

subplot(223) 
plot(T(l:k)',Torq); 
titlefCommand  Torques  vs  Time'); 
xlabel('Time  (sec)');ylabel('Command  Torques'); 

%  Cost  Function 

subplot(222) 

pIot(T,J(l,:));title('Cost  vs  Time'); 

xlabel('Time(sec)');\iabel('J=abs(Uwh)'); 

subplot(224) 

plot(T,JInt),title('lntegrated  Cost  vs  Time'); 

xlabeI(Time(sec)');ylabelCJInt'); 

if  MetaFlag 

meta  main 
end 
if  DocFlag 

meta  doc3 
end 
pause 

ifTfR>0 
disp(") 

dispCREFERENCE  TORQUE  HISTORY'): 

if  WheelFlag 

disp('      Time    UO    ULS    ULE    ULW    URS    URE    URW), 

else 

dispO      Time       ULS        ULE        ULW        URS        URE        URW); 

end 

Table4  =  [TRef  TorqRef], 

disp(Table4) 
end 
pause 
disp(") 

disp('COMMAND  TORQUE  HISTORY'); 
if  WheelFlag 

dispC     Time   UO    ULS    ULE   ULW    URS    URE   URW); 
else 


140 


dispC     Time       ULS       ULE       ULW        URS       URE       URW); 
end 
Table5  =  [T(l:k)Torq']; 

disp(Table5) 
pause 

Table6  =  [T(l:k)J(l,:),JInt]; 

disp("); 

disp('COST  FUNCTION  HISTORY'), 

disp('      Time         J      JIntAbs     JIntSqr'); 

disp(  Table6); 

pause 

%  Angular  Momentum 
k  =  length(T); 
for  n  =  1  :k 

[Hs]  =  AngMo(Ls,Ms,CMs,Is,X(n,l:8),X(n,9:16)); 

if  n  =  1 

HHist  =  Hs; 

else 

HHist  =  [HHist;  Hs]; 

end 
end 
clg 

subplot(221); 

plol(T,HHist(:,l  :6));title('Angular  Momentum  of  Pieees  vs  Time'); 
xlabel(Time  (sec)'),ylabel('Ang  Momentum  (N-m-sec)'); 
subplot(223); 

plot(T,HHist(:,7));title('Total  Angular  Momentum  vs  Time'), 
\label(Time  (sec)');ylabel('Ang  Momentum  (N-m-see)'). 
%  Kinetie  Energy 
form  =  Ik 

if  AMatFlag 

[M,G,A,Adol,B]  =  MatxFix(Ls,Ms,CMs,Is,X(m,  1 :8),X(m,9: 16),AngConst); 

else 

[M,G,A,Adot,B]  =  Matx(Ls,Ms,CMs,Is,X(m,l  :8),X(m,9: 16),AngConst); 

end 

LHSTot(m)  =  0; 

RI  ISTot(m)  =  0; 

ResTot(m)  =  0; 

for  n=  1:8 

LHSTot(m)  =  LHSTot(m)  +  LHS(n,m); 
RHSTot(m)  =  RHSTot(m)  +  RHS(n.m), 

end 

ResTot(m)  =  LHSTot(m)  -  RHSTot(m); 

KE(m)  =  U.5*X(m,9: 1 6)*M*X(m,9: 16)'; 
end 

subplot(224) 

plot(T,KE),title('Kinetic  Energy  vs  Time'), 
xlabeK'Time  (sec)');ylabel('KE  (kg  mA2/sA2)'); 
%  Compare  wheel  torque  to  change  in  total  angular  momentum 
Hd  =  J(2,:)'; 
subplot(222) 

plot(T(  1  :k)',Torq(  1 ,:  ),T(  1  :k)',Hd); 

title( 'Compare  Wheel  Torque  to  Change  in  Ang.  Mom.'); 
xlabel(Time  (sec)'); 
pause 
if  MetaFlag 

meta  main 
end 
if  DocFlag 
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mcta  doc4 
end 
%pause 

clg 
subplot(221) 

plot(T,Res);title('Residuals  of  Equations'), 

\lahcl(Time  (sec)');ylabel('Ll  IS-RHS'); 

subplot(223) 

plot(T,ResTot);title(Total  Residuals'); 

xlabel(*Time(sec)');ylabelCLHS-RHS'); 

%  Constraints:  see  if  A*Qdot  =  0  is  satisfied 

subplot(222) 

plot(T(l:k),Aqdot(l,:),T(l:k),Aqdot(2,:),T(l;k).Aqdot(3,:), 

T(l:k),Aqdot(4,:)); 
[dum  1  ,dum2J  =  size(Aqdot); 
if  duml  ==5 

hold  on 

plot(T(l:k),Aqdol(5,:)); 

hold  off 
end 

litleCCon.siraints:  A*Qdot  vs  Time'); 
xlabeK'Time  (sec)');ylabel('A*Qdot'); 
if  MetaFlag 

meta  main 
end 
if  DocFlag 

meta  doc5 
end 
pause 

end      %  End  of  OutFlag  branch 


I.    Matx 


%  Filename  is  'Matx.m' 

%  This  routine  calculates  the  matrices  for  the  dual  arm 

%  spacecraft  EOM  when  it  is  grasping  a  payload.  Each  arm 

%  has  two  links.  This  version  assumes  that  the  centcrbody 

%  is  NOT  fixed.  This  impacts  A  and  Adot. 

function  [M,G,A,Adot,B]  =  Matx(Ls,Ms,CMs,Is,ThsJhdots,AngConst) 

%  OUTPUTS: 

%  M  =  8x8  mass  matrix 

%  G  =  8x1  vector  with  coriolis  and  centripetal  terms 

%  A  =  4x8  constraints  matrix 

%  Adot  =  4x8  derivative  of  constraints  matrix 

%  B  =  Control  influence  matrix 

% 

%  INPUTS: 

%  Ls  =  7x  1  vector  of  lengths  (m) 

%  1  st  element  =  distance  from  origin  to  left  arm  mount 

%  2nd  &  3rd  elements  wrt  left  arm  (from  shoulder  to  wrist) 

%  4th  element  =  payload  length 

%  5th  &  6th  elements  wrt  right  arm  (from  wnst  to  shoulder) 

%  7th  element  =  distance  from  right  arm  mount  to  origin 

%  [L0;L1;L2;LP,R2;R1;R0] 

%  Ms  =  6x1  column  vector  containing  the  masses  (kg) 
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%  1  st  element  =  mass  of  spacecraft  centerbodv 

%  2nd  &  3rd  elements  =  left  ami  (upper  then  lower  ami) 

%  4th  &  5th  elements  =  right  ami  (upper  then  lower  ami) 

%  6th  element  =  pay  load  mass 

%  | MO.  ML  1 ;  ML2;  MR  1 ;  MR2;  MP] 

%  CMs  =  6x1  column  vector  containing  center  of  mass  locations  (m) 

%  [LcO;  LcLl;LcL2;  LcRl,  LcR2,  LcP] 

%  Is  =  6x  1  column  vector  containing  the  moments  of  inertias 

%  about  the  respective  body's  center  of  mass  (kg  mA2) 

%  1  st  element  =  inertia  of  spacecraft  centerbody 

%  2nd  &  3rd  elements  =  left  arm  (upper  then  lower  arm) 

%         4th  &  5th  elements  =  right  arm  (upper  then  lower  arm) 

%         6th  element  =  pay  load  inertia 

%  [10;  IL1;  IL2;  IR'l ;  IR2;  IP] 

%  Ths  =  6  element  vector  containing  the  angles  which  describe 

%  the  configuration  of  the  system. 

%  [ThO;  ThL  1 ;  ThL2;  ThR  1 ;  ThR2;  ThP | 

%  Thdots  =  6  element  vector  containing  the  angle  rates 

%  AngConst  =  2  element  vector  of  arm  mounting  locations 

%  [ThLO;  ThRO] 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  CONVERT  INPUTS  FROM  ARRAYS  TO  SCALARS  %% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Lengths  (m) 

LO  =  Ls(l); 

LI  =Ls(2) 

L2  =  Ls(3) 

LP  =  Ls(4) 

R2  =  Ls(5) 

Rl  =Ls(6) 

RO  =  Ls(7) 

%  Member  masses  (kg) 
MO  =Ms(l); 
ML1  =Ms(2); 
ML2  =  Ms(3); 
MR1  =Ms(4); 
MR2  =  Ms(5); 
MP  =Ms(6); 

%  Center  of  mass  distances  (m) 

LcO  =CMs(l); 

LcLl  =CMs(2); 

LcL2  =  CMs(3); 

LcRl  =CMs(4); 

LcR2  =  CMs(5); 

LcP  =  CMs(6),  %measured  from  left  end 

%  MOI  about  center  of  mass 

10  =Is(l); 

1L1  =Is(2); 

IL2  =  Is(3); 

IR1  =Is(4); 

IR2  =  Is(5); 

IP  =  Is(6); 

%  Angles 
ThO  =Ths(l); 
ThLl=Ths(2); 
ThL2  =  Ths(3); 
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ThR  1  =  Ths(4); 
ThR2  =  Ths(5); 
ThP  =Ths(6); 

%  Angle  Rates 
ThOd  =Thdots(l); 
ThLld  =  Thdots(2); 
ThL2d  -  Thdots(3); 
ThRld  =  Thdots(4); 
ThR2d  =  Thdots(5); 
ThPd  =  Thdots(6); 

%  Arm  mount  locations 
ThLO  =  AngConst(l); 
ThRO  =  AngConst(2); 

%%%%%%%%%%% 

%%  MassMatnx  %% 

%%%%%%%%%%% 

M  =  zeros(8,8); 

M(8,8)  =  MP; 

M(7,7)  =  MP; 

M(6,6)  =  IP; 

M(5,5)  =  IR2  +  MR2*LeR2A2; 

M(5,4)  =  M(5,5)  +  MR2*Rl*LcR2*cos(ThR2); 

M(4,5)  =  M(5,4); 

M(5, 1 )  =  M(4,5)  +  MR2*R0*LcR2*cos(ThR  1  +ThR2); 

M(1,5)  =  M(5,1); 

M(4.4)  =  M(4,5)+IR  1  +MR2*R  1  *LcR2*cos(ThR2)+MR  1  *LcR  1  A2+MR2*R  1 A2, 

M(4,l)=M(4,4)+R0*(MRl*LcRl+MR2*Rl)*cos(ThRl)+MR2*R()*I.cR2*... 

cos(ThRl+ThR2); 
M(1,4)  =  M(4,1); 
M(3,3)  =  IL2  +  ML2*LcL2A2; 
M(  3,2)  =  M(3,3)  +  ML2*L  i  *LcL2*cos(ThL2); 
M(2,3)  =  M(3,2); 

M(3,l )  =  M(3,2)  +  ML2*L0*LcL2*cos(TliL  1+Thl,2); 
M(1,3)  =  M(3,1); 

M(2,2)  =  M(3,2)+ML2*L  1  *LcL2*cos(ThL2)+IL  1 +ML 1  *LcL  1  A2+ML2*L  1 A2; 
M(2,l  )=M(2,2)+L0*(ML  1  *LcL  1  +ML2*L  1  )*eos(ThL  1  )+ML2*L0*LcL2*... 

cos(ThLl+ThL2); 
M(1,2)  =  M(2,1); 

Parti  =I0+M(2,2)+M(4,4)+M0*Lc0A2+(MLl+ML2)*I.()A2+(MRl+MR2)*R0A2) 
Part2  =  2*LO*(MLl*LcLl+ML2*Ll)*cos(ThLl)+2*ML2*LO*LcL2*... 

cos(ThLl+ThL2); 
Part3  =  2*R0*(MRl*LcRl+MR2*Rl)*cos(ThRl)+2*MR2*R0*LcR2*... 

cos(ThRl+ThR2); 
M(l,l)  =  Partl  +  Part2  +  Part3; 

%%%%%% 

%%  G  %% 

%%%%%% 

G  =  zeros(8,l); 

Pt  1  =  -L0*(ThL  1  dA2+2*Th0d*ThL  1  d)*(ML  1  *I,cI ,  1  +ML2*L  1  )*sin(ThL  1 ); 

Pt2  =  -ML2*Ll*LcL2*ThL2d*(2*Th0d+2*ThI.1d+ThL2d)*sin(ThL2); 

Pt3=-ML2*L0*LcL2*(2*Th0d*(ThLld+ThL2d)+(ThLld+ThL2d)A2)*... 

sin(ThLl+ThL2); 
Pt4  =  -R0*(ThR  1  dA2+2*Th0d*ThR  1  d)*(MR  1  *LcR  1  +MR2*R  I  )*sin(ThR  1 ); 
Pt5  =  -MR2*Rl*LcR2*ThR2d*(2*Th0d+2*ThRld+ThR2d)*sin(ThR2); 
Pt6=-MR2*RO*LcR2*(2*ThOd*(ThRld+ThR2d)+(ThRld+ThR2d)A2)*... 

sin(ThRl+ThR2); 
G(l)  =  Ptl  +  Pt2  +  Pt3  +  Pt4  +  Pt5  +  Pt6; 
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I'll  =  L0*Th0dA2*(MLl*LcLl+ML2*Ll)*sin(ThLl); 

Pt2  =  -ML2*Ll*LcL2*ThL2d*(2*ThOd+2*ThLld+ThL2d)*sin(ThL2); 

Pt3  =  ML2*L0*LcL2*Th0dA2*sin(ThLl+ThL2); 

G(2)  =  Ptl  +Pt2  +  Pt3; 

G(3)  =  ML2*LcL2*(Ll*(Th0d+ThLld)A2*sin(ThL2)+L0*Th0dA2*... 

sin(ThLl+ThL2)); 
Ptl  =R0*Th0dA2*(MRl*LcR1+MR2*Rl)*sin(ThR1); 
Pl2  =  -MR2*Rl*LcR2*ThR2d*(2*Th0d+2*ThRld+ThR2d)*sin(ThR2); 
Pt3  =  MR2*R0*LcR2*Th0dA2*sin(ThRl+ThR2); 
G(4)  =  Ptl  +  Pt2  +  Pt3; 
G(5)  =  MR2*LcR2*(Rl*(ThOd+ThRld)A2*sin(ThR2)+RO*ThOdA2*... 

sin(ThRl+ThR2)); 

%%%%%%%%%%%%%% 

%%  Constraints  Matrix  %% 

%%%%%%%%%%%%%% 

%  The  constraint  matrix  comes  from  putting  the  constraint  equations 

%  into  the  Pfaffian  form:  A*qdot  +  AO  =  0.  The  fust  two  constraint 

%  equations  are  found  by  finding  the  x  and  y  components  of  the 

%  Pay  load's  center  of  mass  by  starting  at  the  origin  and  moving 

%  up  the  left  ami    The  second  two  constraint  equations  find  the  x 

%  and  y  components  of  the  Payload's  center  of  mass  bv  starting  at  the 

%  origin  and  moving  to  the  base  of  the  right  arm  and  then 

%  up  the  right  arm.  Differentiating  these  equations  results 

%  in  the  Pfaffian  form  with  AO  =  0. 

A  =  zeros(4,8); 

A(l,7)--1; 

A(2,8)--l; 

A(3,7)  =  -l; 

A(4,8)  =  -l; 

A(l,6)  =  -LcP*sin(ThP); 

A(2,6)  =  LcP*cos(ThP); 

A(3,6)  =  (LP-LcP)*sin(ThP), 

A(4,6)  =  -(LP-LcP)*cos(ThP); 

A(4,5)  =  R2*cos(Th()+ThRO+ThRl+'fhR2); 

A(4,4)  =  A(4,5)  +  R 1  *cos(ThO+ThR()+ThR  1 ); 

A(4,l )  =  A(4,4)  +  R0*cos(Th0+ThR0); 

A(3,5)  -  -R2*sin(Th0+ThR0+ThRl+ThR2); 

A(3,4)  =  A(3,5)  -  R I  *sin(ThO+ThRO+ThR  1 ); 

A(3,l)  =  A(3,4)  -  R0*sin(Th0+ThR0); 

A(2,3)  =  L2*cos(Th0+ThL0+ThLl+ThL2); 

A(2,2)  =  A(2,3j  +  Ll*cos(ThO+ThLO+ThLl); 

A(2,l)  =  A(2,2)  +  L0*cos(Th0+ThL0) 

A(  1 ,3)  =  -L2*sin(ThO+ThLO+Thl .  1  +Thl ,  2 ). 

A(  1 ,2)  =  A(l ,3)  -  L 1  *sinf ThO+Thl ,0+ThL  1 ); 

A(  1,1 )  =  A(  1 ,2)  -  L0*sin(Th0+ThL0); 

Adot  =  zeros(4,8); 

Adot(l,6)  =  -ThPd*LcP*cos(ThP); 

Adotf2,6)  =  -ThPd*LcP*sin(ThP); 

Adot(3,6)  =  ThPd*(LP-LcP)*cos(ThP); 

Adot(4,6)  =  ThPd*(LP-LcP)*sinCThP); 

Adot(4,5)  =  -(ThOd+ThR  ld+ThR2d)*R2*sin(Th()+ThRO+ThR  1  +ThR2); 

Adot(4,4)  =  Adot(4,5)  -  (ThOd+ThRld)*Rl*sin(ThO+ThRO+ThRl); 

Adot(4,l  j  =  Adotf4,4)  -  ThOd*RO*sin(ThO+ThRO); 

Adot(3,5)  =  -(Th0d+ThRld+ThR2d)*R2*cos(Th0+ThR0+ThRl+ThR2); 

Adot(3,4)  =  Adot(3,5)  -  (ThOd+ThR  ld)*R  I  *cos(ThO+ThRO+ThR  1 ); 

Adot(3,l)  =  Adolf 3,4)  -  Th0d*R0*cos(Th0+ThR0); 

Adot(2,3)  =  -(ThOd+ThLld+ThL2d>)*L2*sin('Ih(J+ThI.O+ThLl+ThL2); 

Adotf 2,2)  =  Adot(2,3)  -  (ThOd+ThLld)*Ll*sin(ThO+ThLO+ThLl); 

Adot(2,l )  =  Adotf2,2)  -  Th0d*L0*sinfTh0+ThL0), 
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Adot(l,3)  =  -(ThOd+ThLld+ThL2d)*L2*cos(ThO+ThLO+ThLl+ThL2); 
Adot(I,2)  =  Adot(l,3)  -  (ThOd+ThLld)*Ll*cos(Th()+TiiL()+ThLl); 
Adot(l,l)  =  Adot(l,2)  -  ThOd*LO*cos(ThO+ThLO); 

%%%%%% 
%%  B    %% 
%%%%%% 
B  =  zeros(8,6); 
B(l,3)  =  -1; 
B(l,6)--1; 
B(2,l)=  1; 
B(2,3)  =  -l; 
B(3,2)=  1; 
B(3,3)  =  -l; 
B(4,4)=  1; 
B(4,6)  =  -l; 
B(5,5)  =  1; 
B(5,6)  =  -l; 
B(6,3)=  1; 
B(6.6)=  1; 


J.    MatxFix 

%  Filename  is  'MatxFix. m' 

%  This  routine  calculates  the  matrices  for  the  dual  arm 

%  spacecraft  EOM  when  it  is  grasping  a  payload.  Each  arm 

%  has  two  links.  This  version  assumes  that  the  centerbody 

%  is  fixed.  This  impacts  A  and  Adot. 

function  [M,G,A,Adot,B]  =  Matx(Ls,Ms,CMs,Is,ThsJhdots.AngConst) 

%  OUTPUTS: 

%  M  =  8x8  mass  matrix 

%  G  =  8x1  vector  with  coriohs  and  centripetal  terms 

%  A  =  5x8  constraints  matrix 

%  Adot  =  5x8  derivative  of  constraints  matrix 

%  B  =  Control  influence  matrix 

% 

%  INPUTS: 

%  Ls  =  7x  1  vector  of  lengths  (m) 

%  1  st  element  =  distance  from  origin  to  left  arm  mount 

%  2nd  &  3rd  elements  wrt  left  arm  (from  shoulder  to  wrist) 

%  4th  element  =  payload  length 

%  5th  &  6th  elements  wrt  right  arm  (from  wrist  to  shoulder) 

%  7th  element  =  distance  from  right  arm  mount  to  origin 

%  [L0;L1;L2;LP;R2;R1;R0] 

%  Ms  =  6x1  column  vector  containing  the  masses  (kg) 

%  1  st  element  =  mass  of  spacecraft  centerbody 

%  2nd  &  3rd  elements  =  left  arm  (upper  then  lower  arm) 

%  4th  &  5th  elements  =  right  arm  (upper  then  lower  arm) 

%  6th  element  =  payload  mass 

%  [MO;  ML  1 ;  ML2;  MR  1 ;  MR2;  MP] 

%  CMs  =  6x  1  column  vector  containing  center  of  mass  locations  (m) 

%  [LcO;  LcL  1 ;  LcL2;  LcR  1 ;  LcR2,  LcP] 

%  Is  =  6x1  column  vector  containing  the  moments  of  inertias 

%  about  the  respective  body's  center  of  mass  (kg  mA2) 

%  1  st  element  =  inertia  of  spacecraft  centerbody 

%  2nd  &  3rd  elements  =  left  arm  (upper  then  lower  arm) 

%         4th  &  5th  elements  =  right  arm  (upper  then  lower  arm) 
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%         6th  element  =  pay  load  inertia 
%  [10;  IL1;  II.2;  IR1,  IR2,  IP] 

%  Ths  =  6  element  vector  containing  the  angles  which  describe 

%  the  configuration  of  the  system 

%  [ThO;  ThL  1 ;  ThL2;  ThR  1 ;  ThR2.  ThP] 

%  Thdots  =  6  element  vector  containing  the  angle  rates 

%  AngConst  =  2  element  vector  of  ami  mounting  locations 

%  [ThLO;  ThROJ 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  CONVERT  INPUTS  FROM  ARRAYS  TO  SCAI.ARS  %% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Lengths  (m) 

L0  =  Ls(l); 

L 1  =  Ls(2) 

L2  =  Ls(3) 

LP  =  Ls(4) 

R2  =  Ls(5) 

R 1  =  Ls(6) 

R0  =  Ls(7) 

%  Member  masses  (kg) 
MO  =Ms(l); 
ML1  =Ms(2); 
ML2  =  Ms(3); 
MR1  =Ms(4); 
MR2  =  Ms(5), 
MP  =Ms(6); 

%  Center  of  mass  distances  (m) 

LcO  =CMs(l); 

LcLl  =CMs(2); 

LcL2  =  CMs(3); 

LcRl  =CMs(4); 

LcR2  =  CMs(5); 

LcP  =  CMs(6);  %measured  from  left  end 

%  MOI  about  center  of  mass 

10  =Is(l); 

1L1  =Is(2); 

IL2  =  Is(3); 

IR1  =Is(4); 

IR2  =  Is(5); 

IP  =Is(6); 

%  Angles 
ThO  =Ths(l); 
ThL  1  =  Ths(2); 
ThL2  =  Ths(3); 
ThRl  =Ths(4); 
ThR2  =  Ths(5); 
ThP  =Ths(6); 

%  Angle  Rates 
ThOd  =Thdots(l); 
ThLld  =  Thdots(2); 

ThL2d  =  Thdots(3); 
ThRld  =  Thdots(4); 
ThR2d  =  Thdots(5); 
ThPd  =Thdots(6), 
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%  Arm  mount  locations 
ThLO  =  AngConst(l); 
ThRO  =  AngConst(2); 

%%%%%%%%%%% 

%%  Mass  Matrix  %% 

%%%%%%%%%%% 

M  =  zeros(8,8); 

M(8,8)  =  MP; 

Mt7,7)  =  MP; 

M(6,6)  =  IP; 

M(5,5)  =  IR2  +  MR2*LcR2A2; 

M(5,4)  =  M(5,5)  +  MR2*R1  *LcR2*cos(ThR2); 

M(4,5)  =  M(5,4); 

M(5, 1 )  =  M(4,5)  +  MR2*R0*LcR2*cos(ThR  1  +ThR2); 

M(1,5)  =  M(5,1); 

M(4,4)  =  M(4,5)+IR1  +MR2*R  1  *LcR2*cos(ThR2)+MR  1  *LcR  1  A2+MR2*R  1 A2; 

M(4,l  )=M(4,4)+R0*(MR  1  *LcR  I  +MR2*R  1  )*cos(ThR  1  )+MR2*R0*LcR2*... 

cos(ThRl+ThR2); 
M(1,4)  =  M(4,1); 
M(3,3)  =  IL2  +  ML2*LcL2A2; 
M(3,2)  =  M(3,3)  +  ML2*Ll*LcL2*cos(ThL2). 
M(2,3)  =  M(3,2); 

M(3,l)  =  M(3,2)  +  ML2*L0*LcL2*cos(ThI.  l+T!iL2); 
M(1,3)  =  M(3,1); 

M(2,2)  =  M(3,2)+ML2*L  1  *LcL2*cos(ThL2)+IL  1 +ML 1  *LcL  1  A2+ML2*L  1 A2; 
M(2, 1  )=M(2,2)+L0*(ML  1  *LcL  1  +ML2*L  1  )*cos(ThL  1  )+ML2*L0*LcL2*..  . 

cos(ThLl+ThL2); 
M(1,2)  =  M(2,1); 

Parti  =  I0+M(2,2)+M(4,4)+M0*Lc0A2+(ML  1+ML2)*L0A2+(MR1+MR2)*R0A2; 
Part2  =  2*L0*(MLl*LcLl+ML2*Ll)*cos(ThIJ)+2*ML2*L0*LcL2*... 

cos(ThLl+ThL2); 
Part3  =  2*R0*(MR  1  *LcR  1  +MR2*R  1  )*cos(ThR  1  )+2*MR2*R0*LcR2*..  . 

cos(ThRl+ThR2). 
M(l,l)  =  Parti  +  Part2  +  Part3; 

%%%%%% 

%%  G  %% 

%%%%%% 

G  =  zeros(8,l); 

Pt  1  =  -L0*(ThL  1  dA2+2*ThOd*ThL  1  d)*(ML  1  *LcL  1 +MI  ,2*L  1  )*sinf  ThL  1 ); 

Pl2  =  -ML2*Ll*LcL2*ThL2d*(2*ThOd+2*ThLld+ThI.2d)*sin(ThL2); 

Pt3=-ML2*LO*LcL2*(2*Th()d*(ThLld+TliL,2d)+(ThLld+ThL2d)A2)*... 

sin(ThLl+ThL2); 
Pt4  =  -R0*(ThR  1  dA2+2*Th0d*ThR  1  d)*(MR  1  *I  ,cR  1  +MR2*R  1  )*sin(ThR  1 ); 
Pl5  =  -MR2*Rl*LcR2*ThR2d*(2*Th0d+2*ThRld+ThR2d)*sin(ThR2); 
Pt6=-MR2*R0*LcR2*(2*Th0d*(ThRld+ThR2d)+(ThRld+ThR2d)A2)*... 

sin(ThRl+ThR2); 
G(l)  =  Ptl  +  Pt2  +  Pt3  +  Pt4  +  Pt5  +  Pt6; 
Pi  1  =  L0*Th0dA2*(ML  1  *LcL  1  +ML2*L  1  )*sin(ThI .  1 ). 
Pt2  =  -ML2*Ll*LcL2*ThI.2d*(2*Th0d+2*ThLld+ThL2d)*sin(ThL2); 
Pt3  =  ML2*L0*LcL2*Th0dA2*sin(ThLl+ThL2), 
G(2)  =  Ptl  +Pt2  +  Pt3; 
G(3)  =  ML2*LcL2*(Ll*(ThOd+ThLld)A2*sin(ThL2)+L()*Th()dA2*... 

sin(ThLl+ThL2)); 
Pt  1  =  R0*Th0dA2*(MR  1  *LcRl  +MR2*R  1  )*sin(ThR  I ); 
Pt2  =  -MR2*R  1  *LcR2*ThR2d*(2*Th0d+2*ThR  1  d+ThR2d)*sin(ThR2); 
Pt3  =  MR2*RO*LcR2*ThOdA2*sin(ThRl+ThR2); 
G(4)  =  Ptl  +P12  +  P13; 

G(5)  =  MR2*LcR2*(Rl  *(Th0d+ThRld)A2*sin(ThR2)+R()*Th0dA2*... 
sin(ThRl+ThR2)); 
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%%%%%%%%%%%%%% 

%%  Constraints  Matrix  %% 

%%%%%%%%%%%%%% 

%  The  constraint  matrix  comes  from  putting  the  constraint  equations 

%  into  the  Pfaffian  form:  A*qdot  +  AC)  =  0.  The  fust  two  constraint 

%  equations  are  found  by  finding  the  x  and  y  components  of  the 

%  Payload's  center  of  mass  by  starting  at  the  origin  and  moving 

%  up  the  left  arm    The  second  two  constraint  equations  find  the  x 

%  and  y  components  of  the  Payload's  center  of  mass  by  starting  at  the 

%  origin  and  moving  to  the  base  of  the  right  arm  and  then 

%  up  the  right  arm.  Differentiating  these  equations  results 

%  in  the  Pfaffian  form  with  A0  =  0. 

A  =  zeros(5,8); 

A(5,l)=l; 

A(l,7)  =  -1; 

A(2,8)  =  -l; 

A(3,7)  =  -l; 

A(4,8)  =  -l; 

A(l,6)  =  -LcP*sin(ThP); 

A(2,6)=  LcP*cos(ThP); 

A(3,6)  =  (LP-LcP)*sin(ThP), 

A(4,6)  =  -(LP-LcP)*cos(ThP); 

A(4,5)  -  R2*cos(f  hO+ThRO+ThRl+ThR2); 

A(4,4)  =  A(4,5)  +  R 1  *cos(Th()+ThR0+ThR  1 ); 

A(4,l )  =  A(4,4)  +  R0*cos(Th0+ThR0); 

A(3,5)  =  -R2*sin(Th0+ThR0+ThRl+ThR2); 

A(3,4)  =  A(3,5)  -  Rl  *sin(ThO+ThRO+ThR  1 ); 

A(3,l)  =  A(3,4)  -  R0*sin(Th0+ThR0); 

A(2,3)  =  L2*cos(ThO+ThLO+ThLl+ThL2). 

A(2,2)  =  A(2,3)  +  L 1  *cos(Th()+ThL0+ThL  1 ); 

A(2,l)  =  A(2,2)  +  L0*cos(Th0+ThL0); 

A(l,3)  =  -L2*sin(ThO+ThLO+ThLl+ThL2); 

A(l,2)  =  A(l,3)-Ll*sin(ThCK-ThL0+ThLl); 

A(  1 , 1 )  =  A(  1 ,2)  -  L0*sin(Th0+ThL0); 

Adot  =  zeros(5,8); 

Adol(l,6)  =  -ThPd*LcP*cos(ThP); 

Adot(2,6)  =  -ThPd*LcP*sin(ThP); 

Adot(3,6)  =  ThPd*(LP-LcP)*cos(ThP); 

Adot(4,6)  =  ThPd*(LP-LcP)*sin(ThP); 

Adot(4,5)  = -(ThOd+ThRld+ThR2d)*R2*sin(ThO+ThRO+ThRHThR2). 

Adot(4.4)  =  Adot(4,5)  -  (Th0d+ThRld)*Rl*sin(ThO+ThRO+ThRl). 

Adot(4,l )  =  Adot(4,4)  -  Th0d*R0*sin(Th0+ThR0): 

Adot(3,5)  =  -(ThOd+ThRld+ThR2d)*R2*cos(Th()+ThRO+ThRl+ThR2); 

Adot(3,4)  =  Adot(3,5)  -  (ThOd+ThRld)*Rl*cos(Th()+ThR()+ThRl); 

Adot(3,l)  =  Adot(3,4)  -  Th0d*R0*cos(Th0+ThR0); 

Adot(2,3)  =  -(ThOd+ThLld+ThL2d)*L2*sin(ThO+ThLO+ThL  l+ThL2); 

Adot(2,2)  -  Adot(2,3)  -  (ThOd+ThLld)*Ll*sin(ThO+ThLO+ThLl); 

Adot(2,l)  =  Adot(2,2)  -  ThOd*LO*sin(ThO+ThL(>); 

Adot(  1 ,3)  -  -(ThOd+ThL  1  d+ThL2d)*L2*cos(Th()+ThI ,0+ThL  1  +ThL2); 

Adot(  1 ,2)  =  Adot(  1,3)-  (ThOd+ThL  1  d)*L  1  *cos(ThO+ThLO+ThL  1 ); 

Adot(  1,1)  =  Adot(  1 ,2)  -  Th0d*L0*cos(Th0+ThL0); 

%%%%%% 
%%  B  %% 
%%%%%% 
B  =  zeros(8,6); 
B(l,3)  =  -1; 
B(l,6)  =  -1; 
B(2,l)=  1; 
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B(2,3)  =  - 
B(3,2)  = 
B(3,3)  =  - 
B(4.4)  = 
B(4,6)  =  - 
B(5,5)  = 
B(5,6)  =  - 
B(6,3)  = 
B(6,6)  = 


K.  Ref2 

%  Filename  is  'Ref2.m' 

%  Reference  Maneuver  using  cost  function 

%  This  routine  assumes  that  the  spacecraft  centerbody  is  held  fixed. 

function  [Torques,QRef,QdotRef,Aqdot,J,Cl,C2,C3]  = ... 

Ref2(Ls,Ms,CMs,Is,BoundC,T,Wu,Wc,Coef,ConstMat) 

%  OUTPUTS: 

%  Torques  =  7x1  column  vector  of  torques  that  should  he  applied  at 

%  time  T  if  the  motion  is  to  follow  the  reference  traiectory 

%  exactly.  The  vector  is  arranged  as  [UO;  ULS;  ULE;  ULW;  URS,  URE,  URW] 

%  which  are  the  centerbody  torque  followed  by  the  torques  at  the 

%  shoulder,  elbow,  and  wrist  of  the  left  arm  and  then  the  right  arm 

%  respectively. 

%  QRef  =  8x1  column  vector  of  reference  generalized  coordinates 

%  QdotRef  =  8xi  column  vector  of  reference  generalized  velocities 

%  Aqdot  =  4x1  or  5x1  column  vector  (depends  on  status  of  AMatFlag)  which 

%  check  to  see  if  the  constraint  equation  A*Qdot  =  0  is  satisfied 

%  J  =  scalar  value  of  the  reaction  wheel  torque  absolute  value    This 

%  number  will  be  integrated  to  find  the  value  for  the  cost  function. 

%  Lvapunov  Controller  matrices  (reference  trajectory  values) 

%  CI  =8x7  matrix 

%  C2  =  8x4  or  8x5  (depends  on  status  of  AMatFlag)  matrix 

%  C3  =  8x1  matrix 

% 

%  INPUTS: 

%  Ls  =  7x1  vector  of  lengths  (m) 

%  1st  element  =  distance  from  origin  to  left  arm  mount 

%  2nd  &  3rd  elements  wrt  left  arm  (from  shoulder  toward  wrist) 

%  4th  element  =  payload  length 

%  5th  &  6th  elements  wrt  right  ami  (from  wrist  toward  shoulder) 

%  7th  element  =  distance  from  right  arm  mount  to  origin 

%  [LO;  L 1 ;  L2;  LP;  R2;  R 1 ;  RO] 

%  Ms  =  6x1  column  vector  containing  the  masses  (kg) 

%  1  st  element  =  mass  of  spacecraft  centerbody 

%  2nd  &  3rd  elements  =  mass  of  left  arm  (upper  arm  then  lower  arm) 

%         4th  &  5th  elements  =  mass  of  right  arm  (upper  aim  then  lower  arm) 

%         6th  element  =  payload  mass 

%  [MO;  ML  1 ;  ML2;  MR  1 ;  MR2;  MP] 

%  CMs  =  6x1  column  vector  containing  center  of  mass  locations 

%  [LcO;  LcL  1 ;  LcL2;  LcR  1 ;  LcR2;  LcP] 

%  Is  =  6x1  column  vector  containing  the  moments  of  inertias  about  the 

%         respective  body's  center  of  mass  (kg  mA2) 

%  1  st  element  =  inertia  of  spacecraft  centerbody 

%         2nd  &  3rd  elements  =  inertia  of  left  arm  (upper  arm  then  lower  arm) 

%         4th  &  5th  elements  =  inertia  of  right  arm  (upper  aim  then  lower  arm) 

%         6th  element  =  payload  inertia 
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%         [10;  IL1;  IL2;  IR1;  IR2;  IP] 

%  BoundC  =  boundry  conditions  for  the  problem  The  first  column 

%  contains  die  initial  x  and  y  component  of  points  0  &  P 

%  respectively,  the  x  component  of  the  right  aim  base,  the 

%  problem  start  time,  and  the  simulation  slop  time.  The  second 

%  column  contains  the  x  and  y  component  of  points  Q  &  P 

%  respectively,  the  x  component  of  the  right  arm  base,  the 

%  stop  time  for  the  ideal  reference  maneuver,  and  a  Hag  to 

%  activate  or  deactivate  the  controller.  The  origin  for  the 

%  x  and  y  components  is  the  base  of  the  left  arm 

%T  =time 

%  Wu  =  6x6  or  7x7  control  torque  cost  weighting  matrix 

%  We  =  8x8  constraint  cost  weighting  matrix 

%  Coef  =  (n-2)xl  column  vector  of  reference  polynomial  coefficients 

%  beginning  with  order  n  coefficient 

%  ConstMat  =  3x(n-2)  matrix  of  coefficients  for  reference  displacement 

%  (row  1),  velocity  (row  2),  and  acceleration  (row  3) 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  CONVERT  INPUTS  FROM  ARRAYS  TO  SCALARS  %% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Lengths  (m) 

L0  =  Ls(l); 

1,1  =Ls(2); 

L2  =  Ls(3); 

LP  =  Ls(4); 

R2  =  Ls(5); 

Rl  =Ls(6); 

R0  =  Ls(7); 

%  Member  masses  (kg) 
MO  =Ms(l); 
ML1  =Ms(2), 
ML2  =  Ms(3); 
MR1  =Ms(4); 
MR2  -  Ms(5); 
MP  =Ms(6), 

%  Center  of  mass  distances  (m) 

LcO  =CMs(l), 

LcLl  =CMs(2); 

LcL2  =  CMs(3); 

LcRl  =CMs(4); 

LcR2  =  CMs(5); 

LcP  =  CMs(6),  %measured  from  left  end 

%  MOI  about  center  of  mass 

10  =Is(l); 

IL 1  =  Is(2); 

IL2  =  Is(3); 

IR1  =Is(4); 

IR2  =  ls(5); 

IP  =Is(6); 

%  Initial  and  final  locations  of  third  link 

%  Point  Q  is  at  Node  3  (joint  between  Links  2  &  3) 

%  Point  P  is  at  Node  4  (joint  between  Links  3  &  4) 

QxO  =  BoundC(  1,1);    QyO  =  BoundC(  1,2), 

PxO  =  BoundC(2,l);    PyO  =  BoundC(2,2); 

Qxf  =  BoundC(3, 1 );     Qyf  =  BoundC(3,2), 

Pxf  -  BoundC(4, 1 );      Pyf  =  BoundC(4,2); 


151 


%  Arms  mount  locations  wrt  spacecraft  centerbody  coordinate  frame  (rad) 
ThLO  -  BoundC(5,l),  ThRO  =  BoundC(5,2). 

%  Reference  Maneuver  Start  and  Stop  Times 
TO  =BoundC(6,l);    Tf  =  BoundC(6,2); 

%  Constraints  Matrix  Flag 
AMatFlag  =  BoundC(8,l); 

%  Centerbody  Reaction  Wheel  Flag 
WheelFlag  =  BoundC(8,2); 

%  Centerbody  Initial  and  Final  Conditions 
ThOO  =  BoundC(9,l); 
Th0f=BoundC(9,2); 

%  Number  of  equations  in  the  cost  function  constraint  equations 
EOMFlag  =  BoundC(10,l); 

%  Psuedo-Inverse  Flag 
PInvFlag  =  BoundC(10,2); 

%%%%%%%%%%%%%%%%%%%%%% 

%%  PRELIMINARY  CALCULATIONS  %% 

%%%%%%%%%%%%%%%%%%%%%% 

R2D  =  180/pi,      %  Conversion  from  radians  to  degrees 

%  Total  rotation  of  Pay  load 

ThPO  =  atan2(PyO-Qy'0,PxO-QxO);  %  Initial  angle  of  Payload  (rad) 

ThPf  =  atan2(Pvf-Qyf,Pxf-Qxf);  %  Final  angle  of  Payload  (rad) 

DelTliP  =  ThPf  -  ThPO;  %  Total  delta  angle  of  Payload  (rad) 

%  Initial  and  final  locations  of  Payload  center  of  mass 
XPO  =  QxO  +  (PxO  -  QxO)  *  (LcP/LP); 
YPO  =  QyO  +  (PyO  -  QyO)  *  (LcP/LP); 
XPf  =  Qxf  +  (Pxf  -  QxO  *  (LcP/LP), 
YPf  =  Qyf  +  (Pyf  -  QyO  *  (LcP/LP); 

Tau  =  (T-TO)  /  (Tf-TO);  %  Normalize  time 

%  Function  Weighting  Factors  for  how  the  pa\  load  will  move 

%  These  factors  will  cause  the  velocity  and  acceleration  of 

%  the  payload  coordinates  to  be  zero  at  t  =  0  and  t  =  tf. 

%  They  also  permit  the  displacements  for  the  payload  coordinates 

%  to  match  their  initial  and  fmal  values.  These  weighting 

%  factors  will  also  apply  to  the  centerbody  rotation. 

k  =  length(CoeO; 
for  n=l:k 

CTau(k+l-n)  =  Coef(k+l-n)*TauA(n+2); 

CTaud(k+l-n)  =  Coef(k+l-n)*TauA(n+l), 

CTaudd(k+l-n)  =  Coef(k+l-n)*TauA(n); 
end 

%  Weighting  factors 
W    =ConstMat(l,:)*CTau'; 
Wd  =ConstMat(2,:)*CTaud'; 
Wdd  =  ConstMat(3,:)*CTaudd', 

%  Centerbody  angle,  angular  velocity,  angular  acceleration 
DelTh()  =  ThOf-Th00; 
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ThO  =  ThOO  +  W  *  DelThO;  %  Angle  (rad); 

ThOd  =  Wd  *  DelThO  /  (Tf  -  TO);         %  Velocity  (rad/sec); 

ThOdd  =  Wdd  *  DelThO  /  (Tf  -  TO)A2;  %  Acceleration  (rad/secA2); 

%Th()    =0; 

%ThOd  =  0; 

%ThOdd  =  0; 

%  Save  for  plotting 

QRef(  1 )     =  ThO; 

QdotRef(l)  =ThOd; 

QddotRef(l)  =  ThOdd, 

%  Pay  load  angle,  angular  velocity,  angular  acceleration 
ThP  =  ThPO  +  W  *  DelThP;  %  Angle  (rad) 

ThPd  =  Wd  *  DelThP  /  (Tf  -  TO);         %  Velocity  ( rad/sec) 
ThPdd  =  Wdd  *  DelThP  /  (Tf  -  T0)A2;  %  Acceleration  (rad/secA2) 
%  Save  for  plotting 
QRef(6)      =  ThP; 
QdotRef(6)  =  ThPd; 
QddotRef(6)  =  ThPdd, 

%  Pavload  center  of  mass  position,  velocity,  and  acceleration 

Xc  =  XP0  +  W  *  (XPf  -  XPO); 

Xcd  =  Wd  *  (XPf  -  XPO)  /  (Tf  -  TO); 

Xcdd  =  Wdd  *  (XPf  -  XPO)  /  (Tf  -  T0)A2; 

Yc  =  YPO  +  W  *  (YPf  -  YPO); 

Ycd  =  Wd  *  (YPf  -  YPO)  /  (Tf  -  TO); 

Ycdd  =  Wdd  *  (YPf  -  YPO)  /  (Tf  -  T0)A2, 

%  Save  for  plotting 

QRcf(7)      =  Xc; 

QdotRef(7)  =Xcd; 

QddotRef(7)  =  Xcdd; 

QRef(8)      =  Yc; 

QdotRef(8)  =  Ycd. 

QddotRef(8)  =  Ycdd; 

%  Payload  endpoint  coordinates:  Qx,  Qy,  Px,  Py 

Qx  =  Xc  -  LcP  *  cos(ThP); 

Qy  =  Yc  -  LcP  *  sin(ThP); 

Px  =  Xc  +  (LP  -  LcP)  *  cos(ThP); 

Py  =  Yc  +  (LP  -  LcP)  *  sin(ThP); 

%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  Solve  for  Arm  Angles  Required  by  desired  path  %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%%%%%%%%%% 

%%  LEFT  ARM  %% 

%%%%%%%%%%% 

%  Elbow  is  left  of  line  from  arm  base  to  Q  (RQ ) 

LSx  =  L0  *  cos(ThO  +  ThLO); 

LSy  =  L0  *  sin(ThO  +  ThLO); 

RQ  =  sqrt((Qx-LSx)A2+(Qy-LSy)A2);        %  Length  from  arm  base  to  Q 

Betal  =  atan2(Qy-LSy,Qx-LSx);  %  Angle  from  arm  base  to  RQ 

%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2bc) 

%  Apply  to  find  angle  between  RQ  and  Link  LI 

Num  =  LlA2  +  RQA2-L2A2; 

Den  =  2*  LI  *  RQ; 

Beta2  =  acos(Num/Den);  %  Angle  from  RQ  to  Link  1 

ThL  1  =  (Beta  1  +  Beta2)  -  (ThO  +  ThLO);    %  Theta  1 . 1 

%  Use  law  of  cosines  to  find  the  interior  angle  at  the  elbow 

Num  =  LlA2  +  L2A2-RQA2; 

Den  =  2*  LI  *  L2; 
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Beta.l  =  acos(Num/Den); 
ThL2  =  -(pi-Beta3); 
%Save  for  plotting 
QRef(2)  =  ThLl; 
QRef(3)  -  ThL2; 

%%%%%%%%%%%% 

%%  RIGHT  ARM  %% 

%%%%%%%%%%%% 

%  Elbow  is  right  of  line  from  arm  base  (shoulder)  to  P  (wrist)  (RP) 

RSx  =  RO  *  cos(ThO  +  ThRO); 

RSv  =  RO  *  sin(ThO  +  ThRO); 

RP  =  sqrt((Px-RSx)A2+(Py-RSy)A2);  %  Length  from  arm  base  to  P 

Betal  =  atan2(Py-RSy,Px-RSx);         %  Angle  from  arm  base  to  RP 

%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2bc) 

%  Apply  to  find  angle  between  RP  and  Link  Rl 

Num  =  RlA2  +  RPA2-R2A2; 

Den  =  2*Rl  *  RP; 

Beta2  =  acos(Num/Den);  %  Angle  from  Link  Rl  to  RP 

Beta4  =  Betal  -  (ThO  +  ThRO); 

ThRl  =  -(Beta2  -  Beta4); 

Num  =  RlA2  +  R2A2-RPA2; 

Den  =  2*Rl  *  R2; 

Beta3  =  acos(Num/Den); 

ThR2  =  pi  -  Beta3; 

%  Save  for  plotting 

QRef(4)  =  ThRl; 

QRef(5)  =  ThR2; 


%%%%%0/o%%%%%%0/o%%%%%%%%%%%%%%%%%%%%%%%% 

%%  Solve  for  Arm  Angle  Rates  &  Accelerations  required  by  desired  path  %% 

%%%%%%%%%%%%%%%0/o%%%0/o0/o%%%%%%%%%0/o%0/o%%%% 

%%%%%%%%%%% 

%%  LEFT  ARM  %% 

%%%%%%%%%%% 

%  [Qxd;  Qyd]  =  [Hl]*ThOd  +  [H2]*Thd 

%  Qxd  &  Qyd  are  x  &  y  components  of  point  Q  inertial  velocity. 

%  Thd  =  [Thl,  1  dot;  ThL2dot] 

%  H  matrices  are  made  from  expressing  the  x  &  v  components  of  Q  in 

%  terms  of  LO,  ThO,  ThLO,  LI,  ThLl,  L2,  and  Tlil,2 

%  Qx=LO*cos(ThO+ThLO)+L  1  *cos(ThO+ThLO+ ThL  1  )+L2*cos(ThO+... 

%  ThL0+ThLl+ThL2) 

%  Qv-LO*sin(ThO+ThLO)+L  1  *sm(ThO+ThLO+TliL  1  )+L2*sin(ThO+... 

%         ThL0+ThLl+ThL2) 

%  The  differentiation  of  these  equations  lead  to 

%  [Qxd;  Qyd]  =  [Hl]*ThOd  +  [H2]*Thd  which  can  be  solved  for  Thd 

Qxd  =  Xcd  +  LcP  *  ThPd  *  sin(ThP); 

Qyd  =  Ycd  -  LcP  *  ThPd  *  cos(ThP); 

H'2(l,2)  =  -L2*sin(Th0+ThL0+ThL  l+ThL2); 

H2(  1 , 1 )  =  H2(  1 ,2)  -  L 1  *sin(Th0+ThL0+ThL  1 ); 

112(2,2)  =  L2*cos(Th0+ThL0+ThL  1  +ThL2); 

1 12(2, 1 )  =  H2(2,2)  +  L 1  *cos(ThO+ThLO+ThL  1 ); 

H  1(1,1)=  H2(l,l)-L0*sin(Th0+ThL0); 

111(2,])=  H2(2, 1 )  +  L0*cos(Th0+ThL0); 

Thd  =  inv(H2)  *  ([Qxd;  Qyd]  -  Hl*ThOd); 

%  Angle  rates 

ThLld  =  Thd(l); 

ThL2d  =  Thd(2); 

%  Save  for  plotting 

QdotRef(2)  =  ThLld; 

QdotRef(3)  =  ThL2d; 
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%  Differentiation  of  [Qxd,  Qyd]  -  [Hl]*Th()d  +  |II2|*Thd  leads  lo 

%  [Qxdd.  Qvddl  =  [Hldot]*Th()d+[IU]*Th()dd  +  |I  I2dot|*Thd+|II2]*fhdd 

Qxdd  =  Xedd  +  LcP*(ThPdd*sin(ThP)  +  ThPdA2*cos(ThP)); 

Qydd  =  Ycdd  -  LcP*(ThPdd*cos(ThP)  -  ThPdA2*sin(ThP)); 

I  I2dot(  1 ,2)  =  -L2*(Th0d+ThL  1  d+ThL2d)*eos('f  hO+ThLO+ThL  1  +ThL2); 

H2dot(l,l)=  H2dot(l,2)  -  Ll*(ThOd+ThLld)*cos(ThO+ThLO+ThLl). 

H2dot(2,2)  =  -L2*(Th0d+ThLld+ThL2d)*sin(Th0+ThI.0+ThLl+ThL2); 

I  I2dot(2, 1 )  =  H2dot(2,2)  -  L 1  *(ThOd+ThL  1  d  )*sin(Th(  >+ThL0+ThL  1 ), 

1 I 1  dot(  1,1)=  H2dot(l ,  1 )  -  LO*ThOd*cos(Th()+Thl .<))•. 

1 1 1  dot(2, 1 )  =  H2dot(2, 1 )  -  LO*ThOd*sin(Th()+Thl  -0); 

Thdd  =  inv(H2)*([Qxdd;  Qydd]-H2dot*Thd-[Hldot]*Th0d-[Hl]*Th0dd); 

%  Angle  accelerations 

ThLldd  =  Thdd(l); 

ThL2dd  =  Thdd(2); 

QddotRef(2)  =  ThLldd; 

QddotRef(3)  -  ThL2dd; 

%%%%%%%%%%%% 

%%  RIGHT  ARM  %% 

%%%%%%%%%%%% 

%  The  development  is  similar  to  the  left  ami 

%  Px=R()*cos(ThO+ThRO)+R  1  *cos(ThO+ThR()+ThR  1  )+R2*cos(Th0+... 

%  ThR0+ThRl+ThR2) 

%  Py=R0*sin(Th0+ThR0)+Rl  *sin(ThO+ThRO+ThRl  )+R2*sin(Th0+... 

%  ThR0+ThRl+ThR2) 

%  [Pxd;  Pyd]  =  [Hl]*Th()d  +  [H2]*Thd 

Pxd  =  Xcd  -  (LP  -  LcP)  *  ThPd  *  sin(ThP); 

Pyd  =  Yed  +  (LP  -  LcP)  *  ThPd  *  cos(ThP), 

H2(l,2)  =  -R2*sin(Th0+ThR0+ThRl+ThR2). 

I12(  1 , 1 )  =  H2(  1 ,2)  -  R 1  *sin(ThO+ThRO+ThR  1 ); 

H2(2,2)  =  R2*cos(ThO+ThRO+ThRl+ThR2); 

1 12(2, 1 )  =  H2(2,2)  +  R 1  *cos(ThO+ThRO+ThR  1 ); 

H 1  ( 1 , 1 )  =  H2(  1,1)-  R0*sin(Th0+ThR0); 

H 1  (2, 1 )  =  H2(2, 1 )  +  R0*cos(Th0+ThR0); 

Thd  =  inv(H2)  *  ([Pxd,  Pyd]  -  HlThOd); 

%  Angle  rates 

ThRld  =  Thd(l); 

ThR2d  =  Thd(2); 

%  Save  for  plotting 

QdotRef(4)  =  ThRld; 

QdotRef(5)  =  ThR2d, 

%  [Pxdd;  Pydd]  =  [Hldot]*Th0d+[Hl]*Th0dd  +  [H2dot]*Thd+[H2]*Thdd 

Pxdd  =  Xcdd  -  (LP  -  LcP)*(ThPdd*sm(ThP)  +  ThPdA2*cos(ThP)); 

Pvdd  =  Ycdd  +  (LP  -  LcP)*(ThPdd*cos(ThP)  -  ThPdA2*sin(ThP)); 

H2dot(l  ,2)  =  -R2*(ThOd+ThRld+ThR2d)*cos(Thl)+ThRO+ThRl+ThR2); 

H2dot(  1,1)=  H2dot(l  ,2)  -  R 1  *(Th0d+ThR  1  d)*cos(ThO+ThRO+ThR  1 ); 

H2dot(2,2)  = -R2*(Th0d+ThRld+ThR2d)*sin(ThO+ThR0+ThRl+ThR2); 

H2dot(2,l)  =  H2dot(2,2)  -  Rl*(ThOd+ThRld)*sin(ThO+ThRO+ThRl); 

Hldot(l.l)  =  H2dot(l,l)  -  R0*Th0d*cos(Th0+ThR0); 

Hldot(2,l)  =  H2dot(2,l)  -  R0*Th0d*sin(Th()+ThR0). 

Thdd  =  inv(H2)*([Pxdd;  Pydd]-H2dot*Thd-[Hldot]*Th0d-[Hl]*Th()dd); 

%  Angle  accelerations 

ThRldd  =  Thdd(l); 

ThR2dd  =  Thdd(2); 

QddotRef(4)  =  ThRldd; 

QddotRef(5)  =  ThR2dd; 

%%%%%%%%%%%%%%%%%%% 
%%  Find  needed  control  torques,  u  %% 
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%%%%%%%%%%%%%%%%%%% 

%  EOM:  M*qddot  +  G  =  B*u  +  A'*Lam 

%  Constraint  Eqns:  A*qdot  =  0 

%  Solve  EOM  for  qddot  leads  to 

%     qddot  =  inv(M)*(B*u  +  A'*Lam  -  G) 

%  Differentiate  Constraint  Eqns  gives    Adot*qdot  +  A*qddot  =  0 

%  Substitute  qddot  derived  from  EOM  into  differentiated  constraint 

%  eqns  and  solve  for  Lam 

%     Lam  =  -inv(A*inv(M)*A')*(A*inv(M)*(B*u-G)+Adot*qdot) 

%  Substitute  this  expression  for  Lam  into  ongianl  EOM.  Collect  terms 

%  into  the  form  MTilda*qddot  +  GTilda  =  BTilda*u 

%  where  MTilda  =  M 

%  GTilda  =  G  +  A'*inv(A*inv(M)*A')*(Adot*qdot-A*inv(M)*G) 

%  BTilda  =(l-A,*inv(A*inv(M)*A,)*A*inv(M))*B 

%  The  first  five  resulting  equations  apply  to  the  spacecraft  centerbody 

%  and  arms.  The  final  three  apply  to  the  payload.  The  matrix  form  of 

%  the  last  three  equations  is 

%     MPTilda*QPddot  +  GPTilda  =  BPTilda*u 

%%%%%%%%% 
%%  Matrices  %% 
%%%%%%%%% 
AngConst  =  [ThLO,  ThRO], 
%AMatFlag  =  1 ; 
if  AMatFlag 

|M,G,A,Adot,B]  =  MatxFix(Ls,Ms,CMs,Is,QRef,QdotRef.AngConst); 
else 

|M,G,A,Adot,B]  =  Matx(Ls,Ms,CMs,Is,QRef,QdotRef,AngConst); 
end 

if  WheelFlag 
B7  =  [l;0;0;0;0;0;0;0]; 

B  =  [B7  B]; 
end 

%  If  the  cost  function  is  subject  to  the  constraint  that  the  payload 
%  satisfy  the  reference  motion,  then  three  equations  of  motion  are  used. 
%  To  include  the  centerbody  reference  motion,  use  four  equations  from 
%  the  equations  of  motion 

%%%%%%%%% 
%%  MTilda  %% 
%%%%%%%%% 
if  EOMFlag  =  3         %  Use  only  the  payload  equations 

MPTilda  -  M(6:8,6:8); 
else 

if  EOMFlag  =  5       %  Use  the  spacecraft  equations 
MPTilda  =  M(  1:5, 1:5); 

else  %  Use  all  eight  equations 

MPTilda  =  M, 

end 
end 

%%%%%%%%% 
%%  GTilda  %% 
%%%%%%%%% 
Odot  =  QdotRef , 

GTilda  =  G  +  A'*inv(A*inv(M)*A')*(Adot*Qdol  -  A*inv(M)*G); 
if  EOMFlag  =  3         %  Use  only  the  pavload  equations 
GPTilda  =  GTilda(6:8,l); 
else 
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if  EOMFlag  ==  5       %  Use  the  spaceerafl  equations 

GPTilda  =  GTilda(l:5,l); 
else  %  Use  all  eight  equations 

GPTilda  =  GTilda; 

end 
end 

%%%%%%%%% 

%%  BTilda  %% 

%%%%%%%%% 

BTilda  =  (eye(8)  -  A'*inv(A*inv(M)*A,)*A*inv(M))*B. 

if  EOMFlag  ==  3         %  Use  only  the  pay  load  equations 

BPTilda  =  BTilda(6:8,:); 
else 

if  EOMFlag  ==  5       %  Use  the  spacecraft  equations 
BPTilda  =  BTilda(  1:5,:); 

else  %  Use  all  eight  equations 

BPTilda  =  BTilda; 

end 
end 

%%%%%%%%% 

%%  Gl  &G2  %% 

%%%%%%%%% 

%  Use  previous  expression  for  Lam  and  regroup  terms  into  the  following 

%  form      A'*  Lam  =  Kl  +  K2*u 

Kl  =  A'*inv(A*inv(M)*A')*(A*inv(M)*G-Adot*Qdot); 

K2  =  -A'*inv(A*inv(M)*A')*A*inv(M)*B; 

%%%%%%%%% 

%%  Torques  %% 

%%%%%%%%% 

%  Torques  are  calculated  to  minimize  the  following  cost  function: 

%      J  =  0.5*[u'*Wu*u  +  Lam,*A*Wc*A,*Lam  +  Tr*Wr*Tr] 

%  Subject  to  the  constraint:  MP*QPddot  +  GPTilda  -  BPTilda*u  =  0 

%  Combine  the  constraint  into  the  cost  function  by  multiplying  the 

%  constraint  eqn  by  another  Lagrange  multiplier.  Gam,  and  adding  that 

%  to  the  cost  function.  Take  the  gradient  with  respect  to  u  results  in 

%     (Wu+K2'*Wc*K2)*u  +  K2'*Wc*K  1  -  BPTilda^Gam  =  0 

%  Solve  for  u 

%     u  =  inv(Wu+K2'*Wc*K2)*(BPTilda,*Gam  -  K2'*Wc*K  1 ) 

%  Substitute  this  into  the  constraint  eqn.  Solve  the  result  for  Ciam 

%    Gam  =  mv(BPTilda*inv(Wu  +  K2'*Wc*K2)*BPTilda')*(MP*QPddot+ 

%  GPTilda+BPTilda*inv(Wu  +  K2'*Wc*K2)*K2'*Wc*Kl) 

%  Substitute  this  expression  into  the  torque  equation,  u. 

Qddot  =  QddotRef ; 

if  EOMFlag  =  3         %  Use  only  the  payload  equations 

QPddot  =  Qddot(6:8,l); 
else 
if  EOMFlag  =  5       %  Use  the  spacecraft  equations 

QPddot  =  Qddot(  1:5,1); 
else  %  Use  all  eight  equations 

QPddot  =  Qddot; 
end 
end 

%%%%%%%%%%%%%%%% 
%%  PSUEDO-INVERSES  %% 
%%%%%%%%%%%%%%%% 
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%  To  avoid  the  problems  with  poorly  conditioned  matrices,  I've  used  the 
%  psuedo-inverse  rather  than  the  traditional  inverse  in  the  next  two 
%  equations. 
ifPlnvFlag 

Parti  =  pinv(Wu  +  K2'*Wc*K2); 

(Jam  =  pinv(BFrUda*Partl*BPTilda')  *  (MPTilda*QPddot  +  GPTilda  +. 
BPTilda*Part  1  *(K2'*Wc*K  1 )); 
else 

Part  1  =  inv(Wu  +  K2'*Wc*K2); 

Gam  =  inv(BPTilda*Partl  *BPTiIda')  *  (MPTilda*QPddot  +  GPTilda  +... 
BPTilda*Part  1  *(K2'*Wc*K  1 )); 
end 

%  Reference  Torques 

Torques  =  Part  1  *(BPTilda'*Gam  -  K2'*  Wc*K  1 ); 

%  Cost  Function,  J 
.1  =  abs(Torques(l)); 

%Controller  Info 

Ptl  =  A'*inv(A*inv(M)*A'); 

CI  =  inv(M)*(eye(M)  -  Ptl*A*mv(M))*B; 

C2  =  -inv(M)*Ptl*Adot, 

C3  =  inv(M)*(Ptl*A*inv(M)  -  eye(M))*G; 

%%%%%%%%%%%% 

%%  DEBUG  INFO  %% 

%%%%%%%%%%%% 

%%  Are  constraint  equations,  A*qdot=0,  satisfied? 

Aqdot  =  A*QdotRef ; 


L.   RefMin2 

%  Filename  is  'RefMin2.m' 

%  Reference  Maneuver  using  cost  function 

%  This  routine  is  used  by  "MainOpt.m"  to  find  the  optimal  combination 

%  of  reference  trajectory  polynomial  coefficients. 

%  Version  2  uses  the  rate  of  change  of  angular  momentum  to  find 

%  the  wheel  torque. 

function  [Joptl,Jopt2]  =  RefMin2(T,Ls,Ms,CMs,Is,BoundC,Wu,Wc,Coef)ConstMat) 

%  OUTPUTS: 

%  Jopt  =  absolute  value  of  the  reaction  wheel  torque.  This  is  the  cost 

%         function  value  for  purposes  of  optimizing  the  reference 

%         trajectory  polynomial  coefficients.  Jopt  I  will  be  integrated  by 

%         odemin.m  while  Jopt2  is  the  same  value  but  won't  be  integrated. 

% 

%  INPUTS: 

%  T  =  time  (sec) 

%  Ls  =  7x1  vector  of  lengths  (m) 

%  1st  element  =  distance  from  origin  to  left  ami  mount 

%  2nd  &  3rd  elements  wrt  left  arm  (from  shoulder  toward  wrist) 

%  4th  element  =  pay  load  length 

%  5lh  &  6th  elements  wrt  right  arm  (from  wrist  toward  shoulder) 

%  7th  element  =  distance  from  right  arm  mount  to  origin 

%  [L0;L1;L2;LP;R2;R1;R0] 

%  Ms  =  6x1  column  vector  containing  the  masses  (kg) 

%  1  st  element  =  mass  of  spacecraft  centerbody 
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%  2nd  &  3rd  elements  =  mass  of  left  arm  (upper  arm  then  lower  ami) 

%  4th  &  5th  elements  =  mass  of  right  ami  (upper  ami  then  lower  arm) 

%  6th  element  =  pay  load  mass 

%  |M(),  ML  1 ;  ML2;  MRU  MR2;  MP] 

%  CMs  =  6x1  column  vector  containing  center  of  mass  locations 

%     [LcO;  LcLl;  LcL2,  LcRl;  LcR2;  LcP] 

%  Is  =  6x  1  column  vector  containing  the  moments  of  inertias  about  the 

%         respective  body's  center  of  mass  (kg  mA2) 

%         1  st  element  =  inertia  of  spacecraft  eenterbody 

%         2nd  &  3rd  elements  =  inertia  of  left  ami  (upper  arm  then  lower  ami) 

%         4th  &  5th  elements  =  inertia  of  right  arm  (upper  ami  then  lower  arm) 

%         6th  element  =  payload  inertia 

%         [10;  IL1;  IL2;  IR1;  IR2;  IP] 

%  BoundC  =  boundry  conditions  for  the  problem    The  first  column 

%       contains  the  initial  x  and  y  component  of  points  0  &  P 

%       respectively,  the  x  component  of  the  right  ami  base,  the 

%       problem  start  time,  and  the  simulation  slop  time    The  second 

%       column  contains  the  x  and  y  component  of  points  Q  &  P 

%       respectively,  the  x  component  of  the  right  arm  base,  the 

%       stop  time  for  the  ideal  reference  maneuver,  and  a  flag  to 

%       activate  or  deactivate  the  controller.  The  origin  for  the 

%       x  and  y  components  is  the  base  of  the  left  ami. 

%  Wu  =  6x6  control  torque  cost  weighting  matrix 

%  Wc  =  8x8  constraint  cost  weighting  matrix 

%  Coef  =  (n-2)xl  vector  of  polynomial  reference  trajectory  coefficients 

%  in  descending  order  where  n  is  the  highest  order  coefficient 

%  ConstMat  =  3x(n-2)  matrix  of  coefficients  for  reference  trajectory 

%  displacement  (row  1),  velocity  (row  2)  and  acceleration  (row  3) 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  CONVERT  INPUTS  FROM  ARRAYS  TO  SCALARS  %% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Lengths  (m) 

L0  =  Ls(l); 

LI  =  Ls(2); 

L2  =  Ls(3), 

LP  =  Ls(4), 

R2  =  Ls(5); 

Rl  =Ls(6), 

R0  =  Ls(7); 

%  Member  masses  (kg) 
MO  =Ms(l); 
ML1  =Ms(2), 
ML2  =  Ms(3); 
MR1  =Ms(4); 
MR2  =  Ms(5); 
MP  =Ms(6); 

%  Center  of  mass  distances  (m) 

LcO  =CMs(l); 

LcLl  =CMs(2); 

LcL2  =  CMs(3); 

LcRl  =CMs(4); 

LcR2  =  CMs(5), 

LcP  =  CMs(6);  %measured  from  left  end 

%  MOI  about  center  of  mass 
10  =ls(l); 
II.  1  =Is(2); 
IL2  =  Is(3); 
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IR1  =  Is(4); 
IR2  =  Is(5); 
IP  =  Is(6); 

%  Initial  and  final  locations  of  third  link 

%  Point  Q  is  at  Node  3  (joint  between  Links  2  &  3) 

%  Point  P  is  at  Node  4  (joint  between  Links  3  &  4) 

QxO  =  BoundC(l,l);    QyO  =  BoundC(l,2); 

PxO  =  BoundC(2,l);     PyO  =  BoundC(2,2); 

Qxf  =  BoundC(3,l);     Qyf  =  BoundC(3,2); 

Pxf  =  BoundC(4, 1 );      Pyf  =  BoundC(4,2); 

%  Arms  mount  locations  wrt  spacecraft  centerbodv  coordinate  frame  (rad) 
ThLO  =  BoundC(5,l);  ThRO  =  BoundC(5,2); 

%  Reference  Maneuver  Start  and  Stop  Times 
TO  =  BoundC(6,l);    Tf  =  BoundC(6,2); 

%  Constraints  Matrix  Flag 
AMatFlag  =  BoundC(8,l); 

%  Cenlerbody  Reaction  Wheel  Flag 
WheelFlag  =  BoundC(8,2); 

%  Cenlerbody  Initial  and  Final  Conditions 
ThOO  =  BoundC(9,l); 
Th0f=BoundC(9,2); 

%  Number  of  equations  in  the  cost  function  constraint  equations 
HOMFlag  =  BoundC(10,l); 

%  Psuedo-Inverse  Flag 
PlnvFlag  =  BoundC(10,2), 

%%%%%%%%%%%%%%%%%%%%%% 
%%  PRELIMINARY  CALCULATIONS  %% 

%%%%%%%%%%%%%%%°/0%%%%%% 

R2D  =  180/pi;      %  Conversion  from  radians  to  degrees 

%  Total  rotation  of  Payload 

ThPO  -  atan2(PyO-QyO,PxO-QxO);  %  Initial  angle  of  Payload  (rad) 

ThPf  =  atan2(Pvf-Q\'f,Pxf-Qxf);  %  Final  angle  of  Payload  (rad ) 

DelThP  =  ThPf  -  ThPO;  %  Total  delta  angle  of  Payload  (rad) 

%  Initial  and  final  locations  of  Payload  center  of  mass 
XPO  =  QxO  +  (PxO  -  QxO)  *  (LcP/LP); 
YPO  =  QyO  +  (PyO  -  QyO)  *  (LcP/LP); 
XPf  =  Qxf  +  (Pxf  -  Qxf)  *  (LcP/LP), 
YPf  =  Qyf  +  (Pyf  -  QyO  *  (LcP/LP); 

Tau  -  (T-TO)  /  (Tf-TO);  %  Normalize  time 

%  Function  Weighting  Factors  for  how  the  payload  will  move 
%  These  factors  will  cause  the  angular  velocity  and  angular 
%  acceleration  of  the  payload  to  be  zero  at  t  =  0  and  t  =  tf 
%  They  also  permit  the  payload  angle  to  match  its  initial 
%  and  final  values.  These  weighting  factors  will  also  apply 
%  to  the  translational  motion  of  the  payload  center  of  mass 

k  =  length(Coef); 
for  n=l:k 
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CTau(k+l-n)  =  Coef(k+l-n)*TauA(n+2); 

CTaud(k+l-n)  =  Coef(k+l-n)*TauA(n+l); 

CTaudd(k+l-n)  =  Coef(k+l-n)*TauA(n); 
end 

W    =  ConstMat(l,:)*CTau'; 
Wd  =  ConstMat(2,:)*CTaud'; 
Wdd  =  ConstMat(3,:)*CTaudd'; 

%  Centerbody  angle,  angular  velocity,  angular  acceleration 

DelThO  =  ThOf  -  ThOO; 

ThO  =  ThOO  +  W  *  DelThO;  %  Angle  (rad); 

ThOd  =  Wd  *  DelThO  /  (Tf  -  TO),        %  Velocity  (rad/sec); 

ThOdd  =  Wdd  *  DelThO  /  (Tf  -  TO)A2;  %  Acceleration  (rad/secA2); 

%  Save  for  plotting 

QRef(  1 )      =  ThO; 

QdotRef(  1 )  =  ThOd; 

QddotRef(l)  =  ThOdd; 

%  Payload  angle,  angular  velocity,  angular  acceleration 
ThP  =  ThPO  +  W  *  DelThP,  %  Angle  (rad) 

ThPd  =  Wd  *  DelThP  /  (Tf  -  TO);  %  Velocity  (rad/sec) 

ThPdd  =  Wdd  *  DelThP  /  (Tf  -  T0)A2;  %  Acceleration  (rad/secA2) 
%  Save  for  plotting 
QRcf(6)      =  ThP; 
QdotRef(6)  =  ThPd; 
QddotRef(6)  =  ThPdd, 

%  Payload  center  of  mass  position,  velocity,  and  acceleration 

Xc  =  XP0  +  W  *  (XPf  -  XPO); 

Xcd  =  Wd  *  (XPf  -  XPO)  /  (Tf  -  TO); 

Xcdd  =  Wdd  *  (XPf  -  XPO)  /  (Tf  -  T0)A2; 

Yc  =  YPO  +  W  *  (YPf  -  YPO); 

Ycd  =  Wd  *  (YPf  -  YPO)  /  (Tf  -  TO); 

Ycdd  =  Wdd  *  (YPf  -  YPO)  /  (Tf  -  T0)A2; 

%  Save  for  plotting 

QRcf(7)      =  Xc; 

QdotRef(7)  =  Xcd; 

QddotRef(7)  =  Xcdd; 

QRef(8)      =  Yc; 

QdotRef(8)  =  Ycd; 

QddotRef(8)  =  Ycdd; 

%  Payload  endpoint  coordinates:  Qx,  Qy,  Px,  Py 

Qx  =  Xc  -  LcP  *  cos(ThP); 

Qv  =  Yc  -  LcP  *  sin(ThP); 

Px  =  Xc  +  (LP  -  LcP)  *  cos(ThP), 

Py  =  Yc  +  (LP  -  LcP)  *  sin(ThP); 

%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  Solve  for  Arm  Angles  Required  by  desired  path  %% 
%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%%%%%%%%%% 

%%  LEFT  ARM  %% 

%%%%%%%%%%% 

%  Elbow  is  left  of  line  from  arm  base  to  Q  (RQ) 

LSx  =  LO  *  cos(ThO  +  ThLO); 

LSy  =  LO  *  sin(ThO  +  ThLO); 

RQ  =  sqrt((Qx-LSx)A2+(Qy-LSy)A2);        %  Length  from  arm  base  to  Q 

Betal  =  atan2(Qy-LSy,Qx-LSx);  %  Angle  from  arm  base  to  RQ 

%  Law  of  cosmes:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2bc) 

%  Apply  to  find  angle  between  RQ  and  Link  L 1 
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Num  =  LlA2  +  RQA2-L2A2, 

Den  =  2*  LI  *  RQ; 

Beta2  =  acos(Num/Den);  %  Angle  from  RQ  to  Link  1 

Thl .  1  =  (Beta  1  +  Beta2)  -  (ThO  +  ThLO);    %  Theta  L 1 

%  Use  law  of  cosines  to  find  the  interior  angle  at  the  elbow 

Num  =  LlA2  +  L2A2-RQA2; 

Den  =  2  *L1  *  L2; 

Beta3  =  acos(Num/Den); 

ThL2  =  -(pi-Beta3); 

%Save  for  plotting 

QRef(2)  =  ThLl; 

QRef(3)  =  ThL2; 

%%%%%%%%%%%% 

%%  RIGHT  ARM  %% 

%%%%%%%%%%%% 

%  Hlbow  is  right  of  line  from  arm  base  (shoulder)  to  P  (wrist)  (RP) 

RSx  =  RO  *  cos(ThO  +  ThRO); 

RSv  =  RO  *  sin(ThO  +  ThRO); 

RP  =  sqrt((Px-RSx)A2+(Py-RSy)A2);  %  Length  from  arm  base  to  P 

Betal  =  atan2(Py-RSy,Px-RSxj;       %  Angle  from  ami  base  to  RP 

%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2be) 

%  Apply  to  find  angle  between  RP  and  Link  R 1 

Num  =  RlA2  +  RPA2-R2A2; 

Den  =  2*Rl  *  RP; 

Beta2  =  acos(Num/Den);  %  Angle  from  Link  Rl  to  RP 

Beta4  =  Betal  -  (ThO  +  ThRO); 

ThRl  = -(Beta2  -  Beta4); 

Num  =  RlA2  +  R2A2-RPA2; 

Den  =  2*Rl  *  R2; 

Bet  a  3  =  acos(Num/Den); 

ThR2  =  pi-Beta3; 

%  Save  for  plotting 

QRef(4)  =  ThRl; 

QRef(5)  =  ThR2; 

%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  Solve  for  Arm  Angle  Rates  &  Accelerations  %% 
%%  required  by  desired  path  %% 

%0/0%%%%%%%%%%%%%%0/0%%%%%%%0/0% 

%%%%%%%%%%% 

%%  LEFT  ARM  %% 

%%%%%%%%%%% 

%  [Qxd;  Qyd]  =  [Hll*ThOd  +  [II2]*Thd 

%  Qxd  &  Qyd  are  x  &  y  components  of  point  Q  inertial  velocity. 

%  Thd  =  [ThL  ldot;  ThL2dot] 

%  H  matnces  are  made  from  expressing  the  x  &  v  components  of  Q  in 

%  terms  of  LO,  ThO,  ThLO,  L 1 ,  ThL  1 ,  L2,  and  ThL2. 

%  Qx=LO*cos(ThO+ThLO)+L  1  *cos(ThO+Thl  ,0+ThL  1  )+L2*cos(ThO+. 

ThL0+ThLl+ThL2) 
%  Qy=LO*sin(ThO+ThLO)+L  1  *sin(ThO+ThLO+ThL  1  )+L2*sin(ThO+... 

ThL0+ThLl+ThL2) 
%  The  differentiation  of  these  equations  lead  to 
%  [Qxd.  Qvd]  =  [Hl]*ThOd  +  [H2]*Thd  which  can  be  solved  for  Thd 
Qxd  =  Xcd  +  LcP  *  ThPd  *  sin(ThP); 
Qvd  =  Ycd  -  LcP  *  ThPd  *  cos(ThP); 
1 12(  1 ,2)  =  -L2*sin(Th0+ThL0+ThL  1  +ThL2); 
H2(  1 , 1 )  =  H2(  1 ,2)  -  L 1  *sin(ThO+ThLO+ThL  1 ); 
H2(2.2)  =  L2*cos(ThO+ThLO+ThLl+ThL2), 
H2(2, 1 )  =  H2(2,2)  +  L 1  *cos(ThO+ThLO+ThL  1 ); 
H 1  ( 1 , 1 )  =  H2(  1 , 1 )  -  LO*sin(ThO+ThLO); 
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1 1 1  (2. 1 )  =  H2(2, 1 )  +  LO*cos(ThO+ThI .( I ). 
Thd  =  inv(H2)  *  ([Qxd;  Qyd]  -Hl*ThOd); 
%  Angle  rales 
ThLld  =  Thd(l); 
ThL2d  =  Thd(2); 
%  Save  for  plotting 
QdotRef(2)  =  ThLld; 
QdotRef(3)  =  ThL2d; 

%  Differentiation  of  [Qxd,  Qvd]  =  [Hll*ThOd  +  |I  I2]*Thd  leads  to 

%  |Qxdd,  Qvdd]  =  [Hldot]*ThOd+[Hl]*ThOdd  +  [U2dotl*Thd+[H2]*Thdd 

Qxdd  =  Xcdd  +  LcP*(ThPdd*sin(ThP)  +  ThPdA2*cos(  ThP)); 

Qydd  =  Ycdd  -  LcP*(ThPdd*cos(ThP)  -  ThPdA2*sin(ThP)); 

I  I2dot(l,2)  =  -L2*(Th()d+ThIJd+ThL2d)*cos(Th()+ThI,0+ThL  1  +ThL2); 

1 12dot(  1,1)-  H2dot(  1 ,2)  -  L 1  *(Th()d+ThI .  1  d )*cos( ThO+ThLO+ThL  1 ); 

112dot(2,2)  =  -L2*(ThOd+ThLld+ThL2d)*sin(Th()+ThLO+ThLl+Tlil.2). 

H2dot(2,l)=  H2dot(2,2)  -  Ll*(Th0d+ThLld)*sin(Th()+ThLO+ThLl); 

Hldot(l,l)=  H2dot(l,l)-L0*Th0d*cos(Th()+ThI.(>); 

IIldot(2,l)=  H2dot(2,l)  -  LO*ThOd*sin(ThO+ThLO). 

Thdd  =  inv(H2)*([Qxdd;  Qydd]-H2dot*Thd-[IIldot)*Th0d-[ini*Th0dd); 

%  Angle  accelerations 

ThLldd  =  Thdd(l); 

ThL2dd  =  Thdd(2); 

QddotRef(2)  =  ThLldd; 

QddotRef(3)  =  ThL2dd. 

%%%%%%%%%%%% 

%%  RIGHT  ARM  %% 

%%%%%%%%%%%% 

%  The  development  is  similar  to  the  left  ami 

%  Px=R0*cos(Th0+ThR0)+Rl*cos(Th0+ThR0+ThRl)+R2*cos(Th0+... 

ThR0+ThRl+ThR2) 
%  Py=RO*sin(ThO+ThRO)+R  1  *sin(ThO+ThR()+ThR  1  )+R2*sin(Th()+... 

ThR0+ThRl+ThR2) 
%  [Pxd,  Pyd]  =  [Hl]*ThOd  +  [H2]*Thd 
Pxd  =  Xcd  -  (LP  -  LcP)  *  ThPd  *  sin(ThP), 
Pvd  =  Ycd  +  (LP  -  LcP)  *  ThPd  *  cos(ThP). 
H'2(l,2)  =  -R2*sin(ThO+ThRO+ThRl+ThR2); 
H2(l,l)=  H2(l,2)-Rl*sin(Th0+ThR0+ThRl). 
H2(2,2)  =  R2*cos(Th0+ThR0+ThRl+ThR2). 
1 12(2, 1 )  =  H2(2,2)  +  R 1  *cos(ThO+ThRO+ThR  1 ); 
HI  (1 , 1 )  =  H2(  1 , 1 )  -  RO*sin(ThO+ThRO); 
H 1  (2, 1 )  =  H2(2, 1 )  +  RO*cos(ThO+ThRO); 
Thd  =  inv(H2)  *  ([Pxd,  PydJ  -  HlThOd); 
%  Angle  rates 
ThRld  =  Thd(l), 
ThR2d  =  Thd(2); 
%  Save  for  plotting 
QdotRef(4)  =  ThRld; 
QdotRef(5)  =  ThR2d; 

%  [Pxdd;  Pydd]  =  [Hldot]*ThOd+[Hl]*Th()dd  +  [H2dot]*Thd+[H2|*Thdd 
Pxdd  =  Xcdd  -  (LP  -  LcP)*(ThPdd*sm(ThP)  +  ThPdA2*cos(ThP)); 
Pydd  =  Ycdd  +  (LP  -  LcP)*(ThPdd*cos(ThP)  -  ThPdA2*sin(ThP)); 
H2dot(l,2)  =  -R2*(Th0d+ThRld+ThR2d)*cos(Th0+ThR0+ThRl+ThR2), 
H2dot(l,l)  =  H2dot(l,2)  -  Rl*(ThOd+ThRld)*cos(ThO+ThRO+ThRl); 
H2dot(2,2)  =  -R2*(Th()d+ThR  1  d+ThR2d)*sin(Th()+ThR0+ThR  1  +ThR2), 
H2dot(2,l)  =  H2dot(2,2)  -  Rl*(ThOd+ThRld)*sm(Th()+ThRO+ThRI); 
H 1  dot(  1,1)=  H2dot(  1,1)-  RO*ThOd*cos(Th(  1+ThRO); 
Hldot(2,l)=  H2dot(2,l)  -RU*ThOd*sin(ThO+ThRO); 
Thdd  =  inv(H2)*([Pxdd;  Pydd]-H2dot*Thd-[Hldot]*Th0d-[Hl]*Th0dd); 
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%  Angle  accelerations 
ThRldd  =  Thdd(l); 
ThR2dd  =  Thdd(2); 
QddotRef(4)  =  ThRldd; 
QddotRef(5)  =  ThR2dd; 

%%%%%%%%%%%%%%%%%%%% 
%%  Find  needed  control  wheel  torque  %% 
%%%%%%%%%%%%%%%%%%%% 
Q      =  QRef; 
Qdot  =  QdotRef; 
Qddot  =  QddotRef ; 

[Hs,  Hdots]  =  AngMo2(Ls,Ms,CMs,Is,Q.Qdot,Qddot); 

%  Cost  Punction,  Jopt 

%  Wheel  torque  is  the  change  in  total  angular  momentum 

%  Jopt  1  is  integrated  while  Jopt  2  is  not 

Joptl  =  abs(Hdots(7)); 

Jopt2  =  Jopt  1 ; 
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